// 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/reorder_program.h"

#include "ceres/parameter_block.h"

#include "ceres/problem_impl.h"

#include "ceres/program.h"

#include "ceres/sized_cost_function.h"

#include "ceres/solver.h"

#include "gmock/gmock.h"

#include "gtest/gtest.h"

namespace ceres {

namespace internal {

using std::vector;

// Templated base class for the CostFunction signatures.

template <int kNumResiduals, int N0, int N1, int N2>

class MockCostFunctionBase : public

SizedCostFunction<kNumResiduals, N0, N1, N2> {

 public:

  virtual bool Evaluate(double const* const* parameters,

                        double* residuals,

                        double** jacobians) const {

    // Do nothing. This is never called.

    return true;

  }

};

class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};

class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};

class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};

TEST(_, ReorderResidualBlockNormalFunction) {

  ProblemImpl problem;

  double x;

  double y;

  double z;

  problem.AddParameterBlock(&x, 1);

  problem.AddParameterBlock(&y, 1);

  problem.AddParameterBlock(&z, 1);

  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);

  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);

  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);

  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);

  problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);

  problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);

  ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering;

  linear_solver_ordering->AddElementToGroup(&x, 0);

  linear_solver_ordering->AddElementToGroup(&y, 0);

  linear_solver_ordering->AddElementToGroup(&z, 1);

  Solver::Options options;

  options.linear_solver_type = DENSE_SCHUR;

  options.linear_solver_ordering.reset(linear_solver_ordering);

  const vector<ResidualBlock*>& residual_blocks =

      problem.program().residual_blocks();

  vector<ResidualBlock*> expected_residual_blocks;

  // This is a bit fragile, but it serves the purpose. We know the

  // bucketing algorithm that the reordering function uses, so we

  // expect the order for residual blocks for each e_block to be

  // filled in reverse.

  expected_residual_blocks.push_back(residual_blocks[4]);

  expected_residual_blocks.push_back(residual_blocks[1]);

  expected_residual_blocks.push_back(residual_blocks[0]);

  expected_residual_blocks.push_back(residual_blocks[5]);

  expected_residual_blocks.push_back(residual_blocks[2]);

  expected_residual_blocks.push_back(residual_blocks[3]);

  Program* program = problem.mutable_program();

  program->SetParameterOffsetsAndIndex();

  std::string message;

  EXPECT_TRUE(LexicographicallyOrderResidualBlocks(

                  2,

                  problem.mutable_program(),

                  &message));

  EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size());

  for (int i = 0; i < expected_residual_blocks.size(); ++i) {

    EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]);

  }

}

TEST(_, ApplyOrderingOrderingTooSmall) {

  ProblemImpl problem;

  double x;

  double y;

  double z;

  problem.AddParameterBlock(&x, 1);

  problem.AddParameterBlock(&y, 1);

  problem.AddParameterBlock(&z, 1);

  ParameterBlockOrdering linear_solver_ordering;

  linear_solver_ordering.AddElementToGroup(&x, 0);

  linear_solver_ordering.AddElementToGroup(&y, 1);

  Program program(problem.program());

  std::string message;

  EXPECT_FALSE(ApplyOrdering(problem.parameter_map(),

                             linear_solver_ordering,

                             &program,

                             &message));

}

TEST(_, ApplyOrderingNormal) {

  ProblemImpl problem;

  double x;

  double y;

  double z;

  problem.AddParameterBlock(&x, 1);

  problem.AddParameterBlock(&y, 1);

  problem.AddParameterBlock(&z, 1);

  ParameterBlockOrdering linear_solver_ordering;

  linear_solver_ordering.AddElementToGroup(&x, 0);

  linear_solver_ordering.AddElementToGroup(&y, 2);

  linear_solver_ordering.AddElementToGroup(&z, 1);

  Program* program = problem.mutable_program();

  std::string message;

  EXPECT_TRUE(ApplyOrdering(problem.parameter_map(),

                            linear_solver_ordering,

                            program,

                            &message));

  const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();

  EXPECT_EQ(parameter_blocks.size(), 3);

  EXPECT_EQ(parameter_blocks[0]->user_state(), &x);

  EXPECT_EQ(parameter_blocks[1]->user_state(), &z);

  EXPECT_EQ(parameter_blocks[2]->user_state(), &y);

}

#ifndef CERES_NO_SUITESPARSE

class ReorderProgramForSparseNormalCholeskyUsingSuiteSparseTest :

      public ::testing::Test {

 protected:

  void SetUp() {

    problem_.AddResidualBlock(new UnaryCostFunction(), NULL, &x_;;

    problem_.AddResidualBlock(new BinaryCostFunction(), NULL, &z_, &x_;;

    problem_.AddResidualBlock(new BinaryCostFunction(), NULL, &z_, &y_;;

    problem_.AddResidualBlock(new UnaryCostFunction(), NULL, &z_;;

    problem_.AddResidualBlock(new BinaryCostFunction(), NULL, &x_, &y_;;

    problem_.AddResidualBlock(new UnaryCostFunction(), NULL, &y_;;

  }

  void ComputeAndValidateOrdering(

      const ParameterBlockOrdering& linear_solver_ordering) {

    Program* program = problem_.mutable_program();

    vector<ParameterBlock*> unordered_parameter_blocks =

        program->parameter_blocks();

    std::string error;

    EXPECT_TRUE(ReorderProgramForSparseNormalCholesky(

                    ceres::SUITE_SPARSE,

                    linear_solver_ordering,

                    program,

                    &error));

    const vector<ParameterBlock*>& ordered_parameter_blocks =

        program->parameter_blocks();

    EXPECT_EQ(ordered_parameter_blocks.size(),

              unordered_parameter_blocks.size());

    EXPECT_THAT(unordered_parameter_blocks,

                ::testing::UnorderedElementsAreArray(ordered_parameter_blocks));

  }

  ProblemImpl problem_;

  double x_;

  double y_;

  double z_;

};

TEST_F(ReorderProgramForSparseNormalCholeskyUsingSuiteSparseTest,

       EverythingInGroupZero) {

  ParameterBlockOrdering linear_solver_ordering;

  linear_solver_ordering.AddElementToGroup(&x_, 0);

  linear_solver_ordering.AddElementToGroup(&y_, 0);

  linear_solver_ordering.AddElementToGroup(&z_, 0);

  ComputeAndValidateOrdering(linear_solver_ordering);

}

TEST_F(ReorderProgramForSparseNormalCholeskyUsingSuiteSparseTest,

       ContiguousGroups) {

  ParameterBlockOrdering linear_solver_ordering;

  linear_solver_ordering.AddElementToGroup(&x_, 0);

  linear_solver_ordering.AddElementToGroup(&y_, 1);

  linear_solver_ordering.AddElementToGroup(&z_, 2);

  ComputeAndValidateOrdering(linear_solver_ordering);

}

TEST_F(ReorderProgramForSparseNormalCholeskyUsingSuiteSparseTest,

       GroupsWithGaps) {

  ParameterBlockOrdering linear_solver_ordering;

  linear_solver_ordering.AddElementToGroup(&x_, 0);

  linear_solver_ordering.AddElementToGroup(&y_, 2);

  linear_solver_ordering.AddElementToGroup(&z_, 2);

  ComputeAndValidateOrdering(linear_solver_ordering);

}

TEST_F(ReorderProgramForSparseNormalCholeskyUsingSuiteSparseTest,

       NonContiguousStartingAtTwo) {

  ParameterBlockOrdering linear_solver_ordering;

  linear_solver_ordering.AddElementToGroup(&x_, 2);

  linear_solver_ordering.AddElementToGroup(&y_, 4);

  linear_solver_ordering.AddElementToGroup(&z_, 4);

  ComputeAndValidateOrdering(linear_solver_ordering);

}

#endif  // CERES_NO_SUITESPARSE

}  // namespace internal

}  // namespace ceres