// 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: keir@google.com (Keir Mierle)

#include "ceres/gradient_checking_cost_function.h"

#include <cmath>
#include <cstdint>
#include <memory>
#include <vector>

#include "ceres/cost_function.h"
#include "ceres/local_parameterization.h"
#include "ceres/loss_function.h"
#include "ceres/manifold.h"
#include "ceres/parameter_block.h"
#include "ceres/problem_impl.h"
#include "ceres/program.h"
#include "ceres/random.h"
#include "ceres/residual_block.h"
#include "ceres/sized_cost_function.h"
#include "ceres/types.h"
#include "glog/logging.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"

namespace ceres::internal {

using std::vector;
using testing::_;
using testing::AllOf;
using testing::AnyNumber;
using testing::HasSubstr;

// Pick a (non-quadratic) function whose derivative are easy:
//
//    f = exp(- a' x).
//   df = - f a.
//
// where 'a' is a vector of the same size as 'x'. In the block
// version, they are both block vectors, of course.
template <int bad_block = 1, int bad_variable = 2>
class TestTerm : public CostFunction {
 public:
  // The constructor of this function needs to know the number
  // of blocks desired, and the size of each block.
  TestTerm(int arity, int const* dim) : arity_(arity) {
    // Make 'arity' random vectors.
    a_.resize(arity_);
    for (int j = 0; j < arity_; ++j) {
      a_[j].resize(dim[j]);
      for (int u = 0; u < dim[j]; ++u) {
        a_[j][u] = 2.0 * RandDouble() - 1.0;
      }
    }

    for (int i = 0; i < arity_; i++) {
      mutable_parameter_block_sizes()->push_back(dim[i]);
    }
    set_num_residuals(1);
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const override {
    // Compute a . x.
    double ax = 0;
    for (int j = 0; j < arity_; ++j) {
      for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
        ax += a_[j][u] * parameters[j][u];
      }
    }

    // This is the cost, but also appears as a factor
    // in the derivatives.
    double f = *residuals = exp(-ax);

    // Accumulate 1st order derivatives.
    if (jacobians) {
      for (int j = 0; j < arity_; ++j) {
        if (jacobians[j]) {
          for (int u = 0; u < parameter_block_sizes()[j]; ++u) {
            // See comments before class.
            jacobians[j][u] = -f * a_[j][u];

            if (bad_block == j && bad_variable == u) {
              // Whoopsiedoopsie! Deliberately introduce a faulty jacobian entry
              // like what happens when users make an error in their jacobian
              // computations. This should get detected.
              LOG(INFO) << "Poisoning jacobian for parameter block " << j
                        << ", row 0, column " << u;
              jacobians[j][u] += 500;
            }
          }
        }
      }
    }

    return true;
  }

 private:
  int arity_;
  vector<vector<double>> a_;
};

TEST(GradientCheckingCostFunction, ResidualsAndJacobiansArePreservedTest) {
  srand(5);

  // Test with 3 blocks of size 2, 3 and 4.
  int const arity = 3;
  int const dim[arity] = {2, 3, 4};

  // Make a random set of blocks.
  vector<double*> parameters(arity);
  for (int j = 0; j < arity; ++j) {
    parameters[j] = new double[dim[j]];
    for (int u = 0; u < dim[j]; ++u) {
      parameters[j][u] = 2.0 * RandDouble() - 1.0;
    }
  }

  double original_residual;
  double residual;
  vector<double*> original_jacobians(arity);
  vector<double*> jacobians(arity);

  for (int j = 0; j < arity; ++j) {
    // Since residual is one dimensional the jacobians have the same
    // size as the parameter blocks.
    jacobians[j] = new double[dim[j]];
    original_jacobians[j] = new double[dim[j]];
  }

  const double kRelativeStepSize = 1e-6;
  const double kRelativePrecision = 1e-4;

  TestTerm<-1, -1> term(arity, dim);
  GradientCheckingIterationCallback callback;
  auto gradient_checking_cost_function =
      CreateGradientCheckingCostFunction(&term,
                                         nullptr,
                                         kRelativeStepSize,
                                         kRelativePrecision,
                                         "Ignored.",
                                         &callback);
  term.Evaluate(&parameters[0], &original_residual, &original_jacobians[0]);

  gradient_checking_cost_function->Evaluate(
      &parameters[0], &residual, &jacobians[0]);
  EXPECT_EQ(original_residual, residual);

  for (int j = 0; j < arity; j++) {
    for (int k = 0; k < dim[j]; ++k) {
      EXPECT_EQ(original_jacobians[j][k], jacobians[j][k]);
    }

    delete[] parameters[j];
    delete[] jacobians[j];
    delete[] original_jacobians[j];
  }
}

TEST(GradientCheckingCostFunction, SmokeTest) {
  srand(5);

  // Test with 3 blocks of size 2, 3 and 4.
  int const arity = 3;
  int const dim[arity] = {2, 3, 4};

  // Make a random set of blocks.
  vector<double*> parameters(arity);
  for (int j = 0; j < arity; ++j) {
    parameters[j] = new double[dim[j]];
    for (int u = 0; u < dim[j]; ++u) {
      parameters[j][u] = 2.0 * RandDouble() - 1.0;
    }
  }

  double residual;
  vector<double*> jacobians(arity);
  for (int j = 0; j < arity; ++j) {
    // Since residual is one dimensional the jacobians have the same size as the
    // parameter blocks.
    jacobians[j] = new double[dim[j]];
  }

  const double kRelativeStepSize = 1e-6;
  const double kRelativePrecision = 1e-4;

  // Should have one term that's bad, causing everything to get dumped.
  LOG(INFO) << "Bad gradient";
  {
    TestTerm<1, 2> term(arity, dim);
    GradientCheckingIterationCallback callback;
    auto gradient_checking_cost_function =
        CreateGradientCheckingCostFunction(&term,
                                           nullptr,
                                           kRelativeStepSize,
                                           kRelativePrecision,
                                           "Fuzzy banana",
                                           &callback);
    EXPECT_TRUE(gradient_checking_cost_function->Evaluate(
        &parameters[0], &residual, &jacobians[0]));
    EXPECT_TRUE(callback.gradient_error_detected());
    EXPECT_TRUE(callback.error_log().find("Fuzzy banana") != std::string::npos);
    EXPECT_TRUE(callback.error_log().find(
                    "(1,0,2) Relative error worse than") != std::string::npos);
  }

  // The gradient is correct, so no errors are reported.
  LOG(INFO) << "Good gradient";
  {
    TestTerm<-1, -1> term(arity, dim);
    GradientCheckingIterationCallback callback;
    auto gradient_checking_cost_function =
        CreateGradientCheckingCostFunction(&term,
                                           nullptr,
                                           kRelativeStepSize,
                                           kRelativePrecision,
                                           "Fuzzy banana",
                                           &callback);
    EXPECT_TRUE(gradient_checking_cost_function->Evaluate(
        &parameters[0], &residual, &jacobians[0]));
    EXPECT_FALSE(callback.gradient_error_detected());
  }

  for (int j = 0; j < arity; j++) {
    delete[] parameters[j];
    delete[] jacobians[j];
  }
}

// The following three classes are for the purposes of defining
// function signatures. They have dummy Evaluate functions.

// Trivial cost function that accepts a single argument.
class UnaryCostFunction : public CostFunction {
 public:
  UnaryCostFunction(int num_residuals, int32_t parameter_block_size) {
    set_num_residuals(num_residuals);
    mutable_parameter_block_sizes()->push_back(parameter_block_size);
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const final {
    for (int i = 0; i < num_residuals(); ++i) {
      residuals[i] = 1;
    }
    return true;
  }
};

// Trivial cost function that accepts two arguments.
class BinaryCostFunction : public CostFunction {
 public:
  BinaryCostFunction(int num_residuals,
                     int32_t parameter_block1_size,
                     int32_t parameter_block2_size) {
    set_num_residuals(num_residuals);
    mutable_parameter_block_sizes()->push_back(parameter_block1_size);
    mutable_parameter_block_sizes()->push_back(parameter_block2_size);
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const final {
    for (int i = 0; i < num_residuals(); ++i) {
      residuals[i] = 2;
    }
    return true;
  }
};

// Trivial cost function that accepts three arguments.
class TernaryCostFunction : public CostFunction {
 public:
  TernaryCostFunction(int num_residuals,
                      int32_t parameter_block1_size,
                      int32_t parameter_block2_size,
                      int32_t parameter_block3_size) {
    set_num_residuals(num_residuals);
    mutable_parameter_block_sizes()->push_back(parameter_block1_size);
    mutable_parameter_block_sizes()->push_back(parameter_block2_size);
    mutable_parameter_block_sizes()->push_back(parameter_block3_size);
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const final {
    for (int i = 0; i < num_residuals(); ++i) {
      residuals[i] = 3;
    }
    return true;
  }
};

// Verify that the two ParameterBlocks are formed from the same user
// array and have the same Manifold objects.
static void ParameterBlocksAreEquivalent(const ParameterBlock* left,
                                         const ParameterBlock* right) {
  CHECK(left != nullptr);
  CHECK(right != nullptr);
  EXPECT_EQ(left->user_state(), right->user_state());
  EXPECT_EQ(left->Size(), right->Size());
  EXPECT_EQ(left->Size(), right->Size());
  EXPECT_EQ(left->TangentSize(), right->TangentSize());
  EXPECT_EQ(left->manifold(), right->manifold());
  EXPECT_EQ(left->IsConstant(), right->IsConstant());
}

TEST(GradientCheckingProblemImpl,
     ProblemDimensionsMatchUsingLocalParameterization) {
  // Parameter blocks with arbitrarily chosen initial values.
  double x[] = {1.0, 2.0, 3.0};
  double y[] = {4.0, 5.0, 6.0, 7.0};
  double z[] = {8.0, 9.0, 10.0, 11.0, 12.0};
  double w[] = {13.0, 14.0, 15.0, 16.0};

  ProblemImpl problem_impl;
  problem_impl.AddParameterBlock(x, 3);
  problem_impl.AddParameterBlock(y, 4);
  problem_impl.SetParameterBlockConstant(y);
  problem_impl.AddParameterBlock(z, 5);
  problem_impl.AddParameterBlock(w, 4, new QuaternionParameterization);
  // clang-format off
  problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3),
                                nullptr, x);
  problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4),
                                nullptr, z, y);
  problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5),
                                new TrivialLoss, x, z);
  problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3),
                                nullptr, z, x);
  problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4),
                                nullptr, z, x, y);
  // clang-format on

  GradientCheckingIterationCallback callback;
  auto gradient_checking_problem_impl =
      CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback);

  // The dimensions of the two problems match.
  EXPECT_EQ(problem_impl.NumParameterBlocks(),
            gradient_checking_problem_impl->NumParameterBlocks());
  EXPECT_EQ(problem_impl.NumResidualBlocks(),
            gradient_checking_problem_impl->NumResidualBlocks());

  EXPECT_EQ(problem_impl.NumParameters(),
            gradient_checking_problem_impl->NumParameters());
  EXPECT_EQ(problem_impl.NumResiduals(),
            gradient_checking_problem_impl->NumResiduals());

  const Program& program = problem_impl.program();
  const Program& gradient_checking_program =
      gradient_checking_problem_impl->program();

  // Since we added the ParameterBlocks and ResidualBlocks explicitly,
  // they should be in the same order in the two programs. It is
  // possible that may change due to implementation changes to
  // Program. This is not expected to be the case and writing code to
  // anticipate that possibility not worth the extra complexity in
  // this test.
  for (int i = 0; i < program.parameter_blocks().size(); ++i) {
    ParameterBlocksAreEquivalent(
        program.parameter_blocks()[i],
        gradient_checking_program.parameter_blocks()[i]);
  }

  for (int i = 0; i < program.residual_blocks().size(); ++i) {
    // Compare the sizes of the two ResidualBlocks.
    const ResidualBlock* original_residual_block = program.residual_blocks()[i];
    const ResidualBlock* new_residual_block =
        gradient_checking_program.residual_blocks()[i];
    EXPECT_EQ(original_residual_block->NumParameterBlocks(),
              new_residual_block->NumParameterBlocks());
    EXPECT_EQ(original_residual_block->NumResiduals(),
              new_residual_block->NumResiduals());
    EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(),
              new_residual_block->NumScratchDoublesForEvaluate());

    // Verify that the ParameterBlocks for the two residuals are equivalent.
    for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) {
      ParameterBlocksAreEquivalent(
          original_residual_block->parameter_blocks()[j],
          new_residual_block->parameter_blocks()[j]);
    }
  }
}

TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) {
  // Parameter blocks with arbitrarily chosen initial values.
  double x[] = {1.0, 2.0, 3.0};
  double y[] = {4.0, 5.0, 6.0, 7.0};
  double z[] = {8.0, 9.0, 10.0, 11.0, 12.0};
  double w[] = {13.0, 14.0, 15.0, 16.0};

  ProblemImpl problem_impl;
  problem_impl.AddParameterBlock(x, 3);
  problem_impl.AddParameterBlock(y, 4);
  problem_impl.SetParameterBlockConstant(y);
  problem_impl.AddParameterBlock(z, 5);
  problem_impl.AddParameterBlock(w, 4, new QuaternionManifold);
  // clang-format off
  problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3),
                                nullptr, x);
  problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4),
                                nullptr, z, y);
  problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5),
                                new TrivialLoss, x, z);
  problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3),
                                nullptr, z, x);
  problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4),
                                nullptr, z, x, y);
  // clang-format on

  GradientCheckingIterationCallback callback;
  auto gradient_checking_problem_impl =
      CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback);

  // The dimensions of the two problems match.
  EXPECT_EQ(problem_impl.NumParameterBlocks(),
            gradient_checking_problem_impl->NumParameterBlocks());
  EXPECT_EQ(problem_impl.NumResidualBlocks(),
            gradient_checking_problem_impl->NumResidualBlocks());

  EXPECT_EQ(problem_impl.NumParameters(),
            gradient_checking_problem_impl->NumParameters());
  EXPECT_EQ(problem_impl.NumResiduals(),
            gradient_checking_problem_impl->NumResiduals());

  const Program& program = problem_impl.program();
  const Program& gradient_checking_program =
      gradient_checking_problem_impl->program();

  // Since we added the ParameterBlocks and ResidualBlocks explicitly,
  // they should be in the same order in the two programs. It is
  // possible that may change due to implementation changes to
  // Program. This is not expected to be the case and writing code to
  // anticipate that possibility not worth the extra complexity in
  // this test.
  for (int i = 0; i < program.parameter_blocks().size(); ++i) {
    ParameterBlocksAreEquivalent(
        program.parameter_blocks()[i],
        gradient_checking_program.parameter_blocks()[i]);
  }

  for (int i = 0; i < program.residual_blocks().size(); ++i) {
    // Compare the sizes of the two ResidualBlocks.
    const ResidualBlock* original_residual_block = program.residual_blocks()[i];
    const ResidualBlock* new_residual_block =
        gradient_checking_program.residual_blocks()[i];
    EXPECT_EQ(original_residual_block->NumParameterBlocks(),
              new_residual_block->NumParameterBlocks());
    EXPECT_EQ(original_residual_block->NumResiduals(),
              new_residual_block->NumResiduals());
    EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(),
              new_residual_block->NumScratchDoublesForEvaluate());

    // Verify that the ParameterBlocks for the two residuals are equivalent.
    for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) {
      ParameterBlocksAreEquivalent(
          original_residual_block->parameter_blocks()[j],
          new_residual_block->parameter_blocks()[j]);
    }
  }
}

TEST(GradientCheckingProblemImpl, ConstrainedProblemBoundsArePropagated) {
  // Parameter blocks with arbitrarily chosen initial values.
  double x[] = {1.0, 2.0, 3.0};
  ProblemImpl problem_impl;
  problem_impl.AddParameterBlock(x, 3);
  problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), nullptr, x);
  problem_impl.SetParameterLowerBound(x, 0, 0.9);
  problem_impl.SetParameterUpperBound(x, 1, 2.5);

  GradientCheckingIterationCallback callback;
  auto gradient_checking_problem_impl =
      CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback);

  // The dimensions of the two problems match.
  EXPECT_EQ(problem_impl.NumParameterBlocks(),
            gradient_checking_problem_impl->NumParameterBlocks());
  EXPECT_EQ(problem_impl.NumResidualBlocks(),
            gradient_checking_problem_impl->NumResidualBlocks());

  EXPECT_EQ(problem_impl.NumParameters(),
            gradient_checking_problem_impl->NumParameters());
  EXPECT_EQ(problem_impl.NumResiduals(),
            gradient_checking_problem_impl->NumResiduals());

  for (int i = 0; i < 3; ++i) {
    EXPECT_EQ(problem_impl.GetParameterLowerBound(x, i),
              gradient_checking_problem_impl->GetParameterLowerBound(x, i));
    EXPECT_EQ(problem_impl.GetParameterUpperBound(x, i),
              gradient_checking_problem_impl->GetParameterUpperBound(x, i));
  }
}

}  // namespace ceres::internal