#include "pinocchio/autodiff/cppad.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/core/ref.hpp>
#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/functional.hpp>
#include <boost/variant.hpp>

#include <iostream>

#include <cppad/cppad.hpp>

#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/rnea-derivatives.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/multibody.hpp>
#include <pinocchio/multibody/sample-models.hpp>
#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/eigen-common.hpp"
#include "pinocchio/math.hpp"
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/spatial.hpp"
#include "pinocchio/utils/cast.hpp"
#include "pinocchio/utils/check.hpp"
#include "pinocchio/utils/static-if.hpp"

using namespace CppAD;

int main(void)
{
  using CppAD::AD;
  using namespace pinocchio;

  typedef ModelTpl<AD<double>> ADModel;
  typedef DataTpl<AD<double>> ADData;
  typedef Eigen::Matrix<AD<double>, Eigen::Dynamic, 1> ADVectorXs;

  Model model;
  buildModels::humanoidRandom(model);
  ADModel ad_model = model.cast<AD<double>>();
  ADData ad_data(ad_model);

  int nq = ad_model.nq;
  int nv = ad_model.nv;

  /***************************************************************************
   *                               the model
   **************************************************************************/
  ADVectorXs ad_Q, ad_V, ad_A, ad_Y;

  ad_Q = ADVectorXs(nq);
  ad_Q = pinocchio::neutral(ad_model);
  ad_V = ADVectorXs(nv);
  ad_V.setOnes();
  ad_A = ADVectorXs(nv);
  ad_A.setZero();

  // independent variable vector
  Independent(ad_V);
  ad_Y = pinocchio::rnea(ad_model, ad_data, ad_Q, ad_V, ad_A);
  // the model tape
  ADFun<double> fun(ad_V, ad_Y);

  Eigen::VectorXd dv = Eigen::VectorXd(nv);
  dv.setOnes();
  Eigen::VectorXd dtau_dv = fun.Jacobian(dv);

  /***************************************************************************
   *                    comparison to analytical derivatives
   **************************************************************************/
  Data data(model);
  Eigen::VectorXd Q(nq), V(nv), A(nv);
  Q = pinocchio::neutral(model);
  V.setOnes();
  A.setZero();
  computeRNEADerivatives(model, data, Q, V, A);

  std::cout << "Analytical derivatives" << std::endl;
  std::cout << data.dtau_dv << std::endl;
  std::cout << "Automatic differentiation" << std::endl;
  std::cout << dtau_dv.reshaped(nv, nv).transpose() << std::endl;

  if (data.dtau_dv.isApprox(dtau_dv.reshaped(nv, nv).transpose()))
  {
    std::cout << "Comparison from automatic differention to analytical derivatives succesful"
              << std::endl;
  }
  else
  {
    std::cout << "Comparison from automatic differention to analytical derivatives NOT succesful"
              << std::endl;
  }
}
