#include <iostream>
#include <ceres/autodiff.h>

struct F {
  template<typename T>
  // z0 = x0^2 + x2
  // z1 = x0 * x1 + x1^2 + x2^3
  bool operator()(const T* x, T* z) const {
    z[0] = x[0] * x[0] + x[2];
    z[1] = x[0] * x[1] + x[1] * x[1] + x[2] * x[2] * x[2];
    return true;
  }
};

int main(int argc, char** argv) {
  float* x = new float[3]{ 1.0f, 3.0f, 5.0f };  // parameters
  float* z = new float[2];                      // outputs
  float* J = new float[6];                      // jacobians

  F func;

  // autodiff
  const int num_parameters = 3;
  const int num_outputs = 2;

  ceres::internal::AutoDiff<F, float, num_parameters> ad;
  ad.Differentiate(func, &x, num_outputs, z, &J);

  // outputs
  std::cout << "--- outputs ---" << std::endl;
  for (int i = 0; i < 2; i++) {
    std::cout << z[i] << " ";
  }
  std::cout << std::endl;

  // jacobians
  std::cout << "--- J ---" << std::endl;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 3; j++) {
      std::cout << J[i * 3 + j] << " ";
    }
    std::cout << std::endl;
  }

  return 0;
}