// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// 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)

#ifndef CERES_NO_SUITESPARSE

#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 {

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;
    eliminator.reset(SchurEliminatorBase::Create(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_;

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

#ifndef CERES_NO_PROTOCOL_BUFFERS
TEST_F(VisibilityBasedPreconditionerTest, SchurJacobiStructure) {
  options_.preconditioner_type = SCHUR_JACOBI;
  preconditioner_.reset(
      new VisibilityBasedPreconditioner(*A_->block_structure(), options_));
  EXPECT_EQ(get_num_blocks(), num_camera_blocks_);
  EXPECT_EQ(get_num_clusters(), num_camera_blocks_);
  for (int i = 0; i < num_camera_blocks_; ++i) {
    for (int j = 0; j < num_camera_blocks_; ++j) {
      const string msg = StringPrintf("Camera pair: %d %d", i, j);
      SCOPED_TRACE(msg);
      if (i == j) {
        EXPECT_TRUE(IsBlockPairInPreconditioner(i, j));
        EXPECT_FALSE(IsBlockPairOffDiagonal(i, j));
      } else {
        EXPECT_FALSE(IsBlockPairInPreconditioner(i, j));
        EXPECT_TRUE(IsBlockPairOffDiagonal(i, j));
      }
    }
  }
}

TEST_F(VisibilityBasedPreconditionerTest, OneClusterClusterJacobi) {
  options_.preconditioner_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>()
        .ldlt().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_.preconditioner_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_.preconditioner_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());
}
#endif  // CERES_NO_PROTOCOL_BUFFERS

}  // namespace internal
}  // namespace ceres

#endif  // CERES_NO_SUITESPARSE