#include <filesystem>
#include <rbdl/addons/urdfreader/urdfreader.h>
#include <rbdl/rbdl.h>

casadi::MX fd(RigidBodyDynamics::Model &model,
              const RigidBodyDynamics::Math::VectorNd &Q,
              const RigidBodyDynamics::Math::VectorNd &Qdot,
              const RigidBodyDynamics::Math::VectorNd &Tau) {
  RigidBodyDynamics::Math::VectorNd Qddot(model.dof_count);
  ForwardDynamics(model, Q, Qdot, Tau, Qddot);
  return Qddot;
}

int main() {
  std::string source_dir_path =
      std::filesystem::current_path().parent_path().string();
  std::string urdf_path = source_dir_path + "/cartpole_model.urdf";

  RigidBodyDynamics::Model *model = new RigidBodyDynamics::Model();
  // If set 'floating_base=false', there is no error.
  if (!RigidBodyDynamics::Addons::URDFReadFromFile(urdf_path.c_str(), model,
                                                   /*floating_base*/ true,
                                                   /*verbose*/ true)) {
    std::cerr << "[ERROR] Can not load model [" << urdf_path << "]."
              << std::endl;
    abort();
  }

  std::cout << "Dof: " << model->dof_count << std::endl;

  auto Q_sym = RigidBodyDynamics::Math::VectorNd::sym("Q", model->dof_count);
  auto QDot_sym =
      RigidBodyDynamics::Math::VectorNd::sym("QDot", model->dof_count);
  auto Tau_sym =
      RigidBodyDynamics::Math::VectorNd::sym("Tau", model->dof_count);
  casadi::Function fd_fun = casadi::Function(
      "fd_fun", {Q_sym, QDot_sym, Tau_sym},
      {fd(*model, Q_sym, QDot_sym, Tau_sym)}, {"Q", "QDot", "Tau"}, {"QDDot"});

  delete model;

  return 0;
}

terminate called after throwing an instance of 'casadi::CasadiException'
  what():  .../casadi/core/sparsity_internal.cpp:2195: Assertion "in_range(rr, -size1()+ind1, size1()+ind1)" failed:
Out of bounds error. Got elements in range [8,8], which is outside the range [-8,8).
Aborted (core dumped)