//
// Copyright (c) 2020 INRIA
//
#include "pinocchio/codegen/cppadcg.hpp"
#include "pinocchio/autodiff/cppad.hpp"

#include <Eigen/Cholesky>
#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 <cppad/cg/support/cppadcg_eigen.hpp>

#include <iostream>
#include <string>

#include "pinocchio/algorithm/check-data.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/codegen/cppadcg-algo.hpp"
#include "pinocchio/eigen-common.hpp"
#include "pinocchio/math.hpp"
#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/spatial.hpp"
#include "pinocchio/utils/cast.hpp"
#include "pinocchio/utils/check.hpp"
#include "pinocchio/utils/static-if.hpp"

int main(int argc, const char ** argv)
{
  using namespace pinocchio;
  using namespace Eigen;

  std::string filename =
    EXAMPLE_ROBOT_DATA_MODEL_DIR + std::string("/ur_description/urdf/ur5_robot.urdf");
  if (argc > 1)
    filename = argv[1];

  std::cout << "Opening file: " << filename << std::endl;

  // Load the model
  Model model;
  pinocchio::urdf::buildModel(filename, model);

  CodeGenCRBA<double> crba_code_gen(model);

  // Generate the lib if it does not exist and load it afterwards.
  crba_code_gen.initLib();
  crba_code_gen.compileAndLoadLib(PINOCCHIO_CXX_COMPILER);

  // Use it with a random configuration samples in the bounds of the joint limits
  VectorXd q = randomConfiguration(model);
  crba_code_gen.evalFunction(q);

  // Retrieve the result
  MatrixXd & M = crba_code_gen.M;

  // And make it symmetric if needed
  M.template triangularView<Eigen::StrictlyLower>() =
    M.transpose().template triangularView<Eigen::StrictlyLower>();

  // You can check the result with the classic CRBA
  Data data_check(model);
  crba(model, data_check, q, Convention::WORLD);

  data_check.M.triangularView<Eigen::StrictlyLower>() =
    data_check.M.transpose().triangularView<Eigen::StrictlyLower>();

  const MatrixXd & M_check = data_check.M;
  if (M_check.isApprox(M))
  {
    std::cout << "Super! The two results are the same." << std::endl;
    return 0;
  }
  else
  {
    std::cout << "Not Super! The results do not match." << std::endl;
    return -1;
  }
}
