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

#include "ceres/internal/autodiff.h"

#include "gtest/gtest.h"
#include "ceres/random.h"

namespace ceres {
namespace internal {

template <typename T> inline
T &RowMajorAccess(T *base, int rows, int cols, int i, int j) {
  return base[cols * i + j];
}

// Do (symmetric) finite differencing using the given function object 'b' of
// type 'B' and scalar type 'T' with step size 'del'.
//
// The type B should have a signature
//
//   bool operator()(T const *, T *) const;
//
// which maps a vector of parameters to a vector of outputs.
template <typename B, typename T, int M, int N> inline
bool SymmetricDiff(const B& b,
                   const T par[N],
                   T del,           // step size.
                   T fun[M],
                   T jac[M * N]) {  // row-major.
  if (!b(par, fun)) {
    return false;
  }

  // Temporary parameter vector.
  T tmp_par[N];
  for (int j = 0; j < N; ++j) {
    tmp_par[j] = par[j];
  }

  // For each dimension, we do one forward step and one backward step in
  // parameter space, and store the output vector vectors in these vectors.
  T fwd_fun[M];
  T bwd_fun[M];

  for (int j = 0; j < N; ++j) {
    // Forward step.
    tmp_par[j] = par[j] + del;
    if (!b(tmp_par, fwd_fun)) {
      return false;
    }

    // Backward step.
    tmp_par[j] = par[j] - del;
    if (!b(tmp_par, bwd_fun)) {
      return false;
    }

    // Symmetric differencing:
    //   f'(a) = (f(a + h) - f(a - h)) / (2 h)
    for (int i = 0; i < M; ++i) {
      RowMajorAccess(jac, M, N, i, j) =
          (fwd_fun[i] - bwd_fun[i]) / (T(2) * del);
    }

    // Restore our temporary vector.
    tmp_par[j] = par[j];
  }

  return true;
}

template <typename A> inline
void QuaternionToScaledRotation(A const q[4], A R[3 * 3]) {
  // Make convenient names for elements of q.
  A a = q[0];
  A b = q[1];
  A c = q[2];
  A d = q[3];
  // This is not to eliminate common sub-expression, but to
  // make the lines shorter so that they fit in 80 columns!
  A aa = a*a;
  A ab = a*b;
  A ac = a*c;
  A ad = a*d;
  A bb = b*b;
  A bc = b*c;
  A bd = b*d;
  A cc = c*c;
  A cd = c*d;
  A dd = d*d;
#define R(i, j) RowMajorAccess(R, 3, 3, (i), (j))
  R(0, 0) =  aa+bb-cc-dd; R(0, 1) = A(2)*(bc-ad); R(0, 2) = A(2)*(ac+bd);  // NOLINT
  R(1, 0) = A(2)*(ad+bc); R(1, 1) =  aa-bb+cc-dd; R(1, 2) = A(2)*(cd-ab);  // NOLINT
  R(2, 0) = A(2)*(bd-ac); R(2, 1) = A(2)*(ab+cd); R(2, 2) =  aa-bb-cc+dd;  // NOLINT
#undef R
}

// A structure for projecting a 3x4 camera matrix and a
// homogeneous 3D point, to a 2D inhomogeneous point.
struct Projective {
  // Function that takes P and X as separate vectors:
  //   P, X -> x
  template <typename A>
  bool operator()(A const P[12], A const X[4], A x[2]) const {
    A PX[3];
    for (int i = 0; i < 3; ++i) {
      PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
              RowMajorAccess(P, 3, 4, i, 1) * X[1] +
              RowMajorAccess(P, 3, 4, i, 2) * X[2] +
              RowMajorAccess(P, 3, 4, i, 3) * X[3];
    }
    if (PX[2] != 0.0) {
      x[0] = PX[0] / PX[2];
      x[1] = PX[1] / PX[2];
      return true;
    }
    return false;
  }

  // Version that takes P and X packed in one vector:
  //
  //   (P, X) -> x
  //
  template <typename A>
  bool operator()(A const P_X[12 + 4], A x[2]) const {
    return operator()(P_X + 0, P_X + 12, x);
  }
};

// Test projective camera model projector.
TEST(AutoDiff, ProjectiveCameraModel) {
  srand(5);
  double const tol = 1e-10;  // floating-point tolerance.
  double const del = 1e-4;   // finite-difference step.
  double const err = 1e-6;   // finite-difference tolerance.

  Projective b;

  // Make random P and X, in a single vector.
  double PX[12 + 4];
  for (int i = 0; i < 12 + 4; ++i) {
    PX[i] = RandDouble();
  }

  // Handy names for the P and X parts.
  double *P = PX + 0;
  double *X = PX + 12;

  // Apply the mapping, to get image point b_x.
  double b_x[2];
  b(P, X, b_x);

  // Use finite differencing to estimate the Jacobian.
  double fd_x[2];
  double fd_J[2 * (12 + 4)];
  ASSERT_TRUE((SymmetricDiff<Projective, double, 2, 12 + 4>(b, PX, del,
                                                            fd_x, fd_J)));

  for (int i = 0; i < 2; ++i) {
    ASSERT_EQ(fd_x[i], b_x[i]);
  }

  // Use automatic differentiation to compute the Jacobian.
  double ad_x1[2];
  double J_PX[2 * (12 + 4)];
  {
    double *parameters[] = { PX };
    double *jacobians[] = { J_PX };
    ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate(
        b, parameters, 2, ad_x1, jacobians)));

    for (int i = 0; i < 2; ++i) {
      ASSERT_NEAR(ad_x1[i], b_x[i], tol);
    }
  }

  // Use automatic differentiation (again), with two arguments.
  {
    double ad_x2[2];
    double J_P[2 * 12];
    double J_X[2 * 4];
    double *parameters[] = { P, X };
    double *jacobians[] = { J_P, J_X };
    ASSERT_TRUE((AutoDiff<Projective, double, 12, 4>::Differentiate(
        b, parameters, 2, ad_x2, jacobians)));

    for (int i = 0; i < 2; ++i) {
      ASSERT_NEAR(ad_x2[i], b_x[i], tol);
    }

    // Now compare the jacobians we got.
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < 12 + 4; ++j) {
        ASSERT_NEAR(J_PX[(12 + 4) * i + j], fd_J[(12 + 4) * i + j], err);
      }

      for (int j = 0; j < 12; ++j) {
        ASSERT_NEAR(J_PX[(12 + 4) * i + j], J_P[12 * i + j], tol);
      }
      for (int j = 0; j < 4; ++j) {
        ASSERT_NEAR(J_PX[(12 + 4) * i + 12 + j], J_X[4 * i + j], tol);
      }
    }
  }
}

// Object to implement the projection by a calibrated camera.
struct Metric {
  // The mapping is
  //
  //   q, c, X -> x = dehomg(R(q) (X - c))
  //
  // where q is a quaternion and c is the center of projection.
  //
  // This function takes three input vectors.
  template <typename A>
  bool operator()(A const q[4], A const c[3], A const X[3], A x[2]) const {
    A R[3 * 3];
    QuaternionToScaledRotation(q, R);

    // Convert the quaternion mapping all the way to projective matrix.
    A P[3 * 4];

    // Set P(:, 1:3) = R
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        RowMajorAccess(P, 3, 4, i, j) = RowMajorAccess(R, 3, 3, i, j);
      }
    }

    // Set P(:, 4) = - R c
    for (int i = 0; i < 3; ++i) {
      RowMajorAccess(P, 3, 4, i, 3) =
        - (RowMajorAccess(R, 3, 3, i, 0) * c[0] +
           RowMajorAccess(R, 3, 3, i, 1) * c[1] +
           RowMajorAccess(R, 3, 3, i, 2) * c[2]);
    }

    A X1[4] = { X[0], X[1], X[2], A(1) };
    Projective p;
    return p(P, X1, x);
  }

  // A version that takes a single vector.
  template <typename A>
  bool operator()(A const q_c_X[4 + 3 + 3], A x[2]) const {
    return operator()(q_c_X, q_c_X + 4, q_c_X + 4 + 3, x);
  }
};

// This test is similar in structure to the previous one.
TEST(AutoDiff, Metric) {
  srand(5);
  double const tol = 1e-10;  // floating-point tolerance.
  double const del = 1e-4;   // finite-difference step.
  double const err = 1e-5;   // finite-difference tolerance.

  Metric b;

  // Make random parameter vector.
  double qcX[4 + 3 + 3];
  for (int i = 0; i < 4 + 3 + 3; ++i)
    qcX[i] = RandDouble();

  // Handy names.
  double *q = qcX;
  double *c = qcX + 4;
  double *X = qcX + 4 + 3;

  // Compute projection, b_x.
  double b_x[2];
  ASSERT_TRUE(b(q, c, X, b_x));

  // Finite differencing estimate of Jacobian.
  double fd_x[2];
  double fd_J[2 * (4 + 3 + 3)];
  ASSERT_TRUE((SymmetricDiff<Metric, double, 2, 4 + 3 + 3>(b, qcX, del,
                                                           fd_x, fd_J)));

  for (int i = 0; i < 2; ++i) {
    ASSERT_NEAR(fd_x[i], b_x[i], tol);
  }

  // Automatic differentiation.
  double ad_x[2];
  double J_q[2 * 4];
  double J_c[2 * 3];
  double J_X[2 * 3];
  double *parameters[] = { q, c, X };
  double *jacobians[] = { J_q, J_c, J_X };
  ASSERT_TRUE((AutoDiff<Metric, double, 4, 3, 3>::Differentiate(
      b, parameters, 2, ad_x, jacobians)));

  for (int i = 0; i < 2; ++i) {
    ASSERT_NEAR(ad_x[i], b_x[i], tol);
  }

  // Compare the pieces.
  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 4; ++j) {
      ASSERT_NEAR(J_q[4 * i + j], fd_J[(4 + 3 + 3) * i + j], err);
    }
    for (int j = 0; j < 3; ++j) {
      ASSERT_NEAR(J_c[3 * i + j], fd_J[(4 + 3 + 3) * i + j + 4], err);
    }
    for (int j = 0; j < 3; ++j) {
      ASSERT_NEAR(J_X[3 * i + j], fd_J[(4 + 3 + 3) * i + j + 4 + 3], err);
    }
  }
}

struct VaryingResidualFunctor {
  template <typename T>
  bool operator()(const T x[2], T* y) const {
    for (int i = 0; i < num_residuals; ++i) {
      y[i] = T(i) * x[0] * x[1] * x[1];
    }
    return true;
  }

  int num_residuals;
};

TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
  double x[2] = { 1.0, 5.5 };
  double *parameters[] = { x };
  const int kMaxResiduals = 10;
  double J_x[2 * kMaxResiduals];
  double residuals[kMaxResiduals];
  double *jacobians[] = { J_x };

  // Use a single functor, but tweak it to produce different numbers of
  // residuals.
  VaryingResidualFunctor functor;

  for (int num_residuals = 1; num_residuals < kMaxResiduals; ++num_residuals) {
    // Tweak the number of residuals to produce.
    functor.num_residuals = num_residuals;

    // Run autodiff with the new number of residuals.
    ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate(
        functor, parameters, num_residuals, residuals, jacobians)));

    const double kTolerance = 1e-14;
    for (int i = 0; i < num_residuals; ++i) {
      EXPECT_NEAR(J_x[2 * i + 0], i * x[1] * x[1], kTolerance) << "i: " << i;
      EXPECT_NEAR(J_x[2 * i + 1], 2 * i * x[0] * x[1], kTolerance) << "i: " << i;
    }
  }
}

}  // namespace internal
}  // namespace ceres