/* Small example of using bicubic interpolation in ceres with autodiff */
#include <ceres/ceres.h>
#include <ceres/cubic_interpolation.h>

#include <cassert>

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

struct Residual {
  template <typename T>
  bool operator()(const T* shift, T* residual) const {
    T x = px + shift[0], y = py + shift[1], v;

    interpolator.Evaluate(y, x, &v);
    *residual = v - val;
    return true;
  }
  Residual(const Interpolator& img, double px, double py, double val)
      : interpolator(img), px(px), py(py), val(val) {}
  static ceres::CostFunction* CreateCost(const Interpolator& interpolator,
                                         double px, double py, double val) {
    return new ceres::AutoDiffCostFunction<Residual, 1, 2>(
        new Residual(interpolator, px, py, val));
  }
  const Interpolator& interpolator;
  double px, py, val;
};

int main() {
  Eigen::Array<double, 6, 7, Eigen::RowMajor> img_data;
  img_data <<
      0., 0., 0., 0., 0., 0., 0.,
      0., 1., 0., 0., 0., 0., 0.,
      0., 0., 0., 0., 0., 0., 0.,
      0., 0., 0., 0., 2., 0., 0.,
      0., 0., 0., 0., 0., 0., 0.,
      0., 0., 0., 0., 0., 0., 0.;

  Grid grid(img_data.data(), 0, img_data.rows(), 0, img_data.cols());
  Interpolator interpolator(grid);

  double shift[2] = {1., 1.};
  ceres::Problem problem;
  problem.AddParameterBlock(shift, 2);

  problem.AddResidualBlock(Residual::CreateCost(interpolator, .5, .5, 1.),
                           nullptr, shift);
  problem.AddResidualBlock(Residual::CreateCost(interpolator, 3.5, 2.5, 2.),
                           nullptr, shift);

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << "\n" << summary.FullReport()
            << "\nShift: " << shift[0] << ' ' << shift[1] << '\n';

  assert(std::abs(shift[0] - 0.5) < 1e-3);
  assert(std::abs(shift[1] - 0.5) < 1e-3);
  return 0;
}