// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2015 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)

#include "ceres/visibility_based_preconditioner.h"

#include "Eigen/Dense"
#include "ceres/block_random_access_dense_matrix.h"
#include "ceres/block_random_access_sparse_matrix.h"
#include "ceres/block_sparse_matrix.h"
#include "ceres/casts.h"
#include "ceres/collections_port.h"
#include "ceres/file.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/linear_least_squares_problems.h"
#include "ceres/schur_eliminator.h"
#include "ceres/stringprintf.h"
#include "ceres/types.h"
#include "ceres/test_util.h"
#include "glog/logging.h"
#include "gtest/gtest.h"

namespace ceres {
namespace internal {

// TODO(sameeragarwal): Re-enable this test once serialization is
// working again.

// using testing::AssertionResult;
// using testing::AssertionSuccess;
// using testing::AssertionFailure;

// static const double kTolerance = 1e-12;

// class VisibilityBasedPreconditionerTest : public ::testing::Test {
//  public:
//   static const int kCameraSize = 9;

//  protected:
//   void SetUp() {
//     string input_file = TestFileAbsolutePath("problem-6-1384-000.lsqp");

//     scoped_ptr<LinearLeastSquaresProblem> problem(
//         CHECK_NOTNULL(CreateLinearLeastSquaresProblemFromFile(input_file)));
//     A_.reset(down_cast<BlockSparseMatrix*>(problem->A.release()));
//     b_.reset(problem->b.release());
//     D_.reset(problem->D.release());

//     const CompressedRowBlockStructure* bs =
//         CHECK_NOTNULL(A_->block_structure());
//     const int num_col_blocks = bs->cols.size();

//     num_cols_ = A_->num_cols();
//     num_rows_ = A_->num_rows();
//     num_eliminate_blocks_ = problem->num_eliminate_blocks;
//     num_camera_blocks_ = num_col_blocks - num_eliminate_blocks_;
//     options_.elimination_groups.push_back(num_eliminate_blocks_);
//     options_.elimination_groups.push_back(
//         A_->block_structure()->cols.size() - num_eliminate_blocks_);

//     vector<int> blocks(num_col_blocks - num_eliminate_blocks_, 0);
//     for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
//       blocks[i - num_eliminate_blocks_] = bs->cols[i].size;
//     }

//     // The input matrix is a real jacobian and fairly poorly
//     // conditioned. Setting D to a large constant makes the normal
//     // equations better conditioned and makes the tests below better
//     // conditioned.
//     VectorRef(D_.get(), num_cols_).setConstant(10.0);

//     schur_complement_.reset(new BlockRandomAccessDenseMatrix(blocks));
//     Vector rhs(schur_complement_->num_rows());

//     scoped_ptr<SchurEliminatorBase> eliminator;
//     LinearSolver::Options eliminator_options;
//     eliminator_options.elimination_groups = options_.elimination_groups;
//     eliminator_options.num_threads = options_.num_threads;

//     eliminator.reset(SchurEliminatorBase::Create(eliminator_options));
//     eliminator->Init(num_eliminate_blocks_, bs);
//     eliminator->Eliminate(A_.get(), b_.get(), D_.get(),
//                           schur_complement_.get(), rhs.data());
//   }


//   AssertionResult IsSparsityStructureValid() {
//     preconditioner_->InitStorage(*A_->block_structure());
//     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
//     const vector<int>& cluster_membership = get_cluster_membership();

//     for (int i = 0; i < num_camera_blocks_; ++i) {
//       for (int j = i; j < num_camera_blocks_; ++j) {
//         if (cluster_pairs.count(make_pair(cluster_membership[i],
//                                           cluster_membership[j]))) {
//           if (!IsBlockPairInPreconditioner(i, j)) {
//             return AssertionFailure()
//                 << "block pair (" << i << "," << j << "missing";
//           }
//         } else {
//           if (IsBlockPairInPreconditioner(i, j)) {
//             return AssertionFailure()
//                << "block pair (" << i << "," << j << "should not be present";
//           }
//         }
//       }
//     }
//     return AssertionSuccess();
//   }

//   AssertionResult PreconditionerValuesMatch() {
//     preconditioner_->Update(*A_, D_.get());
//     const HashSet<pair<int, int> >& cluster_pairs = get_cluster_pairs();
//     const BlockRandomAccessSparseMatrix* m = get_m();
//     Matrix preconditioner_matrix;
//     m->matrix()->ToDenseMatrix(&preconditioner_matrix);
//     ConstMatrixRef full_schur_complement(schur_complement_->values(),
//                                          m->num_rows(),
//                                          m->num_rows());
//     const int num_clusters = get_num_clusters();
//     const int kDiagonalBlockSize =
//         kCameraSize * num_camera_blocks_ / num_clusters;

//     for (int i = 0; i < num_clusters; ++i) {
//       for (int j = i; j < num_clusters; ++j) {
//         double diff = 0.0;
//         if (cluster_pairs.count(make_pair(i, j))) {
//           diff =
//               (preconditioner_matrix.block(kDiagonalBlockSize * i,
//                                            kDiagonalBlockSize * j,
//                                            kDiagonalBlockSize,
//                                            kDiagonalBlockSize) -
//                full_schur_complement.block(kDiagonalBlockSize * i,
//                                            kDiagonalBlockSize * j,
//                                            kDiagonalBlockSize,
//                                            kDiagonalBlockSize)).norm();
//         } else {
//           diff = preconditioner_matrix.block(kDiagonalBlockSize * i,
//                                              kDiagonalBlockSize * j,
//                                              kDiagonalBlockSize,
//                                              kDiagonalBlockSize).norm();
//         }
//         if (diff > kTolerance) {
//           return AssertionFailure()
//               << "Preconditioner block " << i << " " << j << " differs "
//               << "from expected value by " << diff;
//         }
//       }
//     }
//     return AssertionSuccess();
//   }

//   // Accessors
//   int get_num_blocks() { return preconditioner_->num_blocks_; }

//   int get_num_clusters() { return preconditioner_->num_clusters_; }
//   int* get_mutable_num_clusters() { return &preconditioner_->num_clusters_; }

//   const vector<int>& get_block_size() {
//     return preconditioner_->block_size_; }

//   vector<int>* get_mutable_block_size() {
//     return &preconditioner_->block_size_; }

//   const vector<int>& get_cluster_membership() {
//     return preconditioner_->cluster_membership_;
//   }

//   vector<int>* get_mutable_cluster_membership() {
//     return &preconditioner_->cluster_membership_;
//   }

//   const set<pair<int, int> >& get_block_pairs() {
//     return preconditioner_->block_pairs_;
//   }

//   set<pair<int, int> >* get_mutable_block_pairs() {
//     return &preconditioner_->block_pairs_;
//   }

//   const HashSet<pair<int, int> >& get_cluster_pairs() {
//     return preconditioner_->cluster_pairs_;
//   }

//   HashSet<pair<int, int> >* get_mutable_cluster_pairs() {
//     return &preconditioner_->cluster_pairs_;
//   }

//   bool IsBlockPairInPreconditioner(const int block1, const int block2) {
//     return preconditioner_->IsBlockPairInPreconditioner(block1, block2);
//   }

//   bool IsBlockPairOffDiagonal(const int block1, const int block2) {
//     return preconditioner_->IsBlockPairOffDiagonal(block1, block2);
//   }

//   const BlockRandomAccessSparseMatrix* get_m() {
//     return preconditioner_->m_.get();
//   }

//   int num_rows_;
//   int num_cols_;
//   int num_eliminate_blocks_;
//   int num_camera_blocks_;

//   scoped_ptr<BlockSparseMatrix> A_;
//   scoped_array<double> b_;
//   scoped_array<double> D_;

//   Preconditioner::Options options_;
//   scoped_ptr<VisibilityBasedPreconditioner> preconditioner_;
//   scoped_ptr<BlockRandomAccessDenseMatrix> schur_complement_;
// };

// TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
//   options_.type = CLUSTER_JACOBI;
//   preconditioner_.reset(
//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));

//   // Override the clustering to be a single clustering containing all
//   // the cameras.
//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
//   for (int i = 0; i < num_camera_blocks_; ++i) {
//     cluster_membership[i] = 0;
//   }

//   *get_mutable_num_clusters() = 1;

//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
//   cluster_pairs.clear();
//   cluster_pairs.insert(make_pair(0, 0));

//   EXPECT_TRUE(IsSparsityStructureValid());
//   EXPECT_TRUE(PreconditionerValuesMatch());

//   // Multiplication by the inverse of the preconditioner.
//   const int num_rows = schur_complement_->num_rows();
//   ConstMatrixRef full_schur_complement(schur_complement_->values(),
//                                        num_rows,
//                                        num_rows);
//   Vector x(num_rows);
//   Vector y(num_rows);
//   Vector z(num_rows);

//   for (int i = 0; i < num_rows; ++i) {
//     x.setZero();
//     y.setZero();
//     z.setZero();
//     x[i] = 1.0;
//     preconditioner_->RightMultiply(x.data(), y.data());
//     z = full_schur_complement
//         .selfadjointView<Eigen::Upper>()
//         .llt().solve(x);
//     double max_relative_difference =
//         ((y - z).array() / z.array()).matrix().lpNorm<Eigen::Infinity>();
//     EXPECT_NEAR(max_relative_difference, 0.0, kTolerance);
//   }
// }


// TEST_F(VisibilityBasedPreconditionerTest, ClusterJacobi) {
//   options_.type = CLUSTER_JACOBI;
//   preconditioner_.reset(
//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));

//   // Override the clustering to be equal number of cameras.
//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
//   cluster_membership.resize(num_camera_blocks_);
//   static const int kNumClusters = 3;

//   for (int i = 0; i < num_camera_blocks_; ++i) {
//     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
//   }
//   *get_mutable_num_clusters() = kNumClusters;

//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
//   cluster_pairs.clear();
//   for (int i = 0; i < kNumClusters; ++i) {
//     cluster_pairs.insert(make_pair(i, i));
//   }

//   EXPECT_TRUE(IsSparsityStructureValid());
//   EXPECT_TRUE(PreconditionerValuesMatch());
// }


// TEST_F(VisibilityBasedPreconditionerTest, ClusterTridiagonal) {
//   options_.type = CLUSTER_TRIDIAGONAL;
//   preconditioner_.reset(
//       new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
//   static const int kNumClusters = 3;

//   // Override the clustering to be 3 clusters.
//   vector<int>& cluster_membership = *get_mutable_cluster_membership();
//   cluster_membership.resize(num_camera_blocks_);
//   for (int i = 0; i < num_camera_blocks_; ++i) {
//     cluster_membership[i] = (i * kNumClusters) / num_camera_blocks_;
//   }
//   *get_mutable_num_clusters() = kNumClusters;

//   // Spanning forest has structure 0-1 2
//   HashSet<pair<int, int> >& cluster_pairs = *get_mutable_cluster_pairs();
//   cluster_pairs.clear();
//   for (int i = 0; i < kNumClusters; ++i) {
//     cluster_pairs.insert(make_pair(i, i));
//   }
//   cluster_pairs.insert(make_pair(0, 1));

//   EXPECT_TRUE(IsSparsityStructureValid());
//   EXPECT_TRUE(PreconditionerValuesMatch());
// }

}  // namespace internal
}  // namespace ceres