// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2021 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.
//
// Bicubic interpolation with automatic differentiation
//
// We will use estimation of 2d shift as a sample problem for bicubic
// interpolation.
//
// Let us define f(x, y) = x * x - y * x + y * y
// And optimize cost function sum_i [f(x_i + s_x, y_i + s_y) - v_i]^2
//
// Bicubic interpolation of f(x, y) will be exact, thus we can expect close to
// perfect convergence

#include <utility>

#include "ceres/ceres.h"
#include "ceres/cubic_interpolation.h"
#include "glog/logging.h"

using Grid = ceres::Grid2D<double>;
using Interpolator = ceres::BiCubicInterpolator<Grid>;

// Cost-function using autodiff interface of BiCubicInterpolator
struct AutoDiffBiCubicCost {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  template <typename T>
  bool operator()(const T* s, T* residual) const {
    using Vector2T = Eigen::Matrix<T, 2, 1>;
    Eigen::Map<const Vector2T> shift(s);

    const Vector2T point = point_ + shift;

    T v;
    interpolator_.Evaluate(point.y(), point.x(), &v);

    *residual = v - value_;
    return true;
  }

  AutoDiffBiCubicCost(const Interpolator& interpolator,
                      Eigen::Vector2d point,
                      double value)
      : point_(std::move(point)), value_(value), interpolator_(interpolator) {}

  static ceres::CostFunction* Create(const Interpolator& interpolator,
                                     const Eigen::Vector2d& point,
                                     double value) {
    return new ceres::AutoDiffCostFunction<AutoDiffBiCubicCost, 1, 2>(
        new AutoDiffBiCubicCost(interpolator, point, value));
  }

  const Eigen::Vector2d point_;
  const double value_;
  const Interpolator& interpolator_;
};

// Function for input data generation
static double f(const double& x, const double& y) {
  return x * x - y * x + y * y;
}

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  // Problem sizes
  const int kGridRowsHalf = 9;
  const int kGridColsHalf = 11;
  const int kGridRows = 2 * kGridRowsHalf + 1;
  const int kGridCols = 2 * kGridColsHalf + 1;
  const int kPoints = 4;

  const Eigen::Vector2d shift(1.234, 2.345);
  const std::array<Eigen::Vector2d, kPoints> points = {
      Eigen::Vector2d{-2., -3.},
      Eigen::Vector2d{-2., 3.},
      Eigen::Vector2d{2., 3.},
      Eigen::Vector2d{2., -3.}};

  // Data is a row-major array of kGridRows x kGridCols values of function
  // f(x, y) on the grid, with x in {-kGridColsHalf, ..., +kGridColsHalf},
  // and y in {-kGridRowsHalf, ..., +kGridRowsHalf}
  double data[kGridRows * kGridCols];
  for (int i = 0; i < kGridRows; ++i) {
    for (int j = 0; j < kGridCols; ++j) {
      // Using row-major order
      int index = i * kGridCols + j;
      double y = i - kGridRowsHalf;
      double x = j - kGridColsHalf;

      data[index] = f(x, y);
    }
  }
  const Grid grid(data,
                  -kGridRowsHalf,
                  kGridRowsHalf + 1,
                  -kGridColsHalf,
                  kGridColsHalf + 1);
  const Interpolator interpolator(grid);

  Eigen::Vector2d shift_estimate(3.1415, 1.337);

  ceres::Problem problem;
  problem.AddParameterBlock(shift_estimate.data(), 2);

  for (const auto& p : points) {
    const Eigen::Vector2d shifted = p + shift;

    const double v = f(shifted.x(), shifted.y());
    problem.AddResidualBlock(AutoDiffBiCubicCost::Create(interpolator, p, v),
                             nullptr,
                             shift_estimate.data());
  }

  ceres::Solver::Options options;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.BriefReport() << '\n';

  std::cout << "Bicubic interpolation with automatic derivatives:\n";
  std::cout << "Estimated shift: " << shift_estimate.transpose()
            << ", ground-truth: " << shift.transpose()
            << " (error: " << (shift_estimate - shift).transpose() << ")"
            << std::endl;

  CHECK_LT((shift_estimate - shift).norm(), 1e-9);
  return 0;
}