// 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)
//         tbennun@gmail.com (Tal Ben-Nun)

#include "ceres/numeric_diff_test_utils.h"

#include <algorithm>
#include <cmath>
#include "ceres/cost_function.h"
#include "ceres/test_util.h"
#include "ceres/types.h"
#include "gtest/gtest.h"


namespace ceres {
namespace internal {

bool EasyFunctor::operator()(const double* x1,
                             const double* x2,
                             double* residuals) const {
  residuals[0] = residuals[1] = residuals[2] = 0;
  for (int i = 0; i < 5; ++i) {
    residuals[0] += x1[i] * x2[i];
    residuals[2] += x2[i] * x2[i];
  }
  residuals[1] = residuals[0] * residuals[0];
  return true;
}

void EasyFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
    const CostFunction& cost_function,
    NumericDiffMethodType method) const {
  // The x1[0] is made deliberately small to test the performance near
  // zero.
  double x1[] = { 1e-64, 2.0, 3.0, 4.0, 5.0 };
  double x2[] = { 9.0, 9.0, 5.0, 5.0, 1.0 };
  double *parameters[] = { &x1[0], &x2[0] };

  double dydx1[15];  // 3 x 5, row major.
  double dydx2[15];  // 3 x 5, row major.
  double *jacobians[2] = { &dydx1[0], &dydx2[0] };

  double residuals[3] = {-1e-100, -2e-100, -3e-100 };

  ASSERT_TRUE(cost_function.Evaluate(&parameters[0],
                                     &residuals[0],
                                     &jacobians[0]));

  double expected_residuals[3];
  EasyFunctor functor;
  functor(x1, x2, expected_residuals);
  EXPECT_EQ(expected_residuals[0], residuals[0]);
  EXPECT_EQ(expected_residuals[1], residuals[1]);
  EXPECT_EQ(expected_residuals[2], residuals[2]);

  double tolerance = 0.0;
  switch (method) {
    default:
    case CENTRAL:
      tolerance = 3e-9;
      break;

    case FORWARD:
      tolerance = 2e-5;
      break;

    case RIDDERS:
      tolerance = 1e-13;
      break;
  }

  for (int i = 0; i < 5; ++i) {
    ExpectClose(x2[i],                    dydx1[5 * 0 + i], tolerance);  // y1
    ExpectClose(x1[i],                    dydx2[5 * 0 + i], tolerance);
    ExpectClose(2 * x2[i] * residuals[0], dydx1[5 * 1 + i], tolerance);  // y2
    ExpectClose(2 * x1[i] * residuals[0], dydx2[5 * 1 + i], tolerance);
    ExpectClose(0.0,                      dydx1[5 * 2 + i], tolerance);  // y3
    ExpectClose(2 * x2[i],                dydx2[5 * 2 + i], tolerance);
  }
}

bool TranscendentalFunctor::operator()(const double* x1,
                                       const double* x2,
                                       double* residuals) const {
  double x1x2 = 0;
  for (int i = 0; i < 5; ++i) {
    x1x2 += x1[i] * x2[i];
  }
  residuals[0] = sin(x1x2);
  residuals[1] = exp(-x1x2 / 10);
  return true;
}

void TranscendentalFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
    const CostFunction& cost_function,
    NumericDiffMethodType method) const {

  struct TestParameterBlocks {
    double x1[5];
    double x2[5];
  };

  std::vector<TestParameterBlocks> kTests =  {
    { { 1.0, 2.0, 3.0, 4.0, 5.0 },  // No zeros.
      { 9.0, 9.0, 5.0, 5.0, 1.0 },
    },
    { { 0.0, 2.0, 3.0, 0.0, 5.0 },  // Some zeros x1.
      { 9.0, 9.0, 5.0, 5.0, 1.0 },
    },
    { { 1.0, 2.0, 3.0, 1.0, 5.0 },  // Some zeros x2.
      { 0.0, 9.0, 0.0, 5.0, 0.0 },
    },
    { { 0.0, 0.0, 0.0, 0.0, 0.0 },  // All zeros x1.
      { 9.0, 9.0, 5.0, 5.0, 1.0 },
    },
    { { 1.0, 2.0, 3.0, 4.0, 5.0 },  // All zeros x2.
      { 0.0, 0.0, 0.0, 0.0, 0.0 },
    },
    { { 0.0, 0.0, 0.0, 0.0, 0.0 },  // All zeros.
      { 0.0, 0.0, 0.0, 0.0, 0.0 },
    },
  };

  for (int k = 0; k < kTests.size(); ++k) {
    double *x1 = &(kTests[k].x1[0]);
    double *x2 = &(kTests[k].x2[0]);
    double *parameters[] = { x1, x2 };

    double dydx1[10];
    double dydx2[10];
    double *jacobians[2] = { &dydx1[0], &dydx2[0] };

    double residuals[2];

    ASSERT_TRUE(cost_function.Evaluate(&parameters[0],
                                       &residuals[0],
                                       &jacobians[0]));
    double x1x2 = 0;
    for (int i = 0; i < 5; ++i) {
      x1x2 += x1[i] * x2[i];
    }

    double tolerance = 0.0;
    switch (method) {
      default:
      case CENTRAL:
        tolerance = 2e-7;
        break;

      case FORWARD:
        tolerance = 2e-5;
        break;

      case RIDDERS:
        tolerance = 3e-12;
        break;
    }

    for (int i = 0; i < 5; ++i) {
      ExpectClose( x2[i] * cos(x1x2),              dydx1[5 * 0 + i], tolerance);
      ExpectClose( x1[i] * cos(x1x2),              dydx2[5 * 0 + i], tolerance);
      ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], tolerance);
      ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], tolerance);
    }
  }
}

bool ExponentialFunctor::operator()(const double* x1,
                                    double* residuals) const {
  residuals[0] = exp(x1[0]);
  return true;
}


void ExponentialFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
    const CostFunction& cost_function) const {
  // Evaluating the functor at specific points for testing.
  std::vector<double> kTests = { 1.0, 2.0, 3.0, 4.0, 5.0 };

  // Minimal tolerance w.r.t. the cost function and the tests.
  const double kTolerance = 2e-14;

  for (int k = 0; k < kTests.size(); ++k) {
    double *parameters[] = { &kTests[k] };
    double dydx;
    double *jacobians[1] = { &dydx };
    double residual;

    ASSERT_TRUE(cost_function.Evaluate(&parameters[0],
                                       &residual,
                                       &jacobians[0]));


    double expected_result = exp(kTests[k]);

    // Expect residual to be close to exp(x).
    ExpectClose(residual, expected_result, kTolerance);

    // Check evaluated differences. dydx should also be close to exp(x).
    ExpectClose(dydx, expected_result, kTolerance);
  }
}

bool RandomizedFunctor::operator()(const double* x1,
                                   double* residuals) const {
  double random_value = static_cast<double>(rand()) /
      static_cast<double>(RAND_MAX);

  // Normalize noise to [-factor, factor].
  random_value *= 2.0;
  random_value -= 1.0;
  random_value *= noise_factor_;

  residuals[0] = x1[0] * x1[0] + random_value;
  return true;
}

void RandomizedFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
    const CostFunction& cost_function) const {
  std::vector<double> kTests = { 0.0, 1.0, 3.0, 4.0, 50.0 };

  const double kTolerance = 2e-4;

  // Initialize random number generator with given seed.
  srand(random_seed_);

  for (int k = 0; k < kTests.size(); ++k) {
    double *parameters[] = { &kTests[k] };
    double dydx;
    double *jacobians[1] = { &dydx };
    double residual;

    ASSERT_TRUE(cost_function.Evaluate(&parameters[0],
                                       &residual,
                                       &jacobians[0]));

    // Expect residual to be close to x^2 w.r.t. noise factor.
    ExpectClose(residual, kTests[k] * kTests[k], noise_factor_);

    // Check evaluated differences. (dy/dx = ~2x)
    ExpectClose(dydx, 2 * kTests[k], kTolerance);
  }
}

}  // namespace internal
}  // namespace ceres