//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2018-2025 INRIA
//

#include "pinocchio/multibody.hpp"

#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/algorithm/model.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"

#include "pinocchio/multibody/sample-models.hpp"
#include "pinocchio/spatial/fwd.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_model_subtree)
{
  Model model;
  buildModels::humanoidRandom(model);

  Model::JointIndex idx_larm1 = model.getJointId("larm1_joint");
  BOOST_CHECK(idx_larm1 < (Model::JointIndex)model.njoints);
  Model::IndexVector & subtree = model.subtrees[idx_larm1];
  BOOST_CHECK(subtree.size() == 6);

  for (size_t joint_id = 0; joint_id < model.joints.size(); ++joint_id)
  {
    const Model::IndexVector & children = model.children[joint_id];
    for (size_t i = 0; i < children.size(); ++i)
    {
      BOOST_CHECK(model.parents[children[i]] == joint_id);
    }
  }

  for (size_t i = 1; i < subtree.size(); ++i)
  {
    BOOST_CHECK(model.parents[subtree[i]] == subtree[i - 1]);
  }

  // Check that i starts subtree[i]
  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
  {
    BOOST_CHECK(model.subtrees[joint_id][0] == joint_id);
  }

  // Check that subtree[0] contains all joint ids
  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
  {
    BOOST_CHECK(model.subtrees[0][joint_id - 1] == joint_id);
  }
}

BOOST_AUTO_TEST_CASE(test_model_get_frame_id)
{
  Model model;
  buildModels::humanoidRandom(model);

  for (FrameIndex i = 0; i < static_cast<FrameIndex>(model.nframes); i++)
  {
    BOOST_CHECK_EQUAL(i, model.getFrameId(model.frames[i].name));
  }
  BOOST_CHECK_EQUAL(model.nframes, model.getFrameId("NOT_A_FRAME"));
}

BOOST_AUTO_TEST_CASE(test_model_support)
{
  Model model;
  buildModels::humanoidRandom(model);
  const Model::IndexVector support0_ref(1, 0);
  BOOST_CHECK(model.supports[0] == support0_ref);

  // Check that i ends supports[i]
  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
  {
    BOOST_CHECK(model.supports[joint_id].back() == joint_id);
    Model::IndexVector & support = model.supports[joint_id];

    size_t current_id = support.size() - 2;
    for (JointIndex parent_id = model.parents[joint_id]; parent_id > 0;
         parent_id = model.parents[parent_id], current_id--)
    {
      BOOST_CHECK(parent_id == support[current_id]);
    }
  }
}

/// Test mimic_joint_support by creating manually a robot and
/// checking the content.
BOOST_AUTO_TEST_CASE(test_model_mimic_joint_support)
{
  Model model;
  // j1 -- j2 -- j3
  //    -- j4 -- j5
  // with j5 mimicking j3
  // with j2 mimicking j1
  model.addJoint(0, JointModelRX(), SE3::Identity(), "j1");
  model.addJoint(
    1, JointModelMimic(JointModelRX(), model.joints[1], 1., 0.), SE3::Identity(), "j2");
  model.addJoint(2, JointModelRX(), SE3::Identity(), "j3");
  model.addJoint(1, JointModelRX(), SE3::Identity(), "j4");
  model.addJoint(
    4, JointModelMimic(JointModelRX(), model.joints[3], 1., 0.), SE3::Identity(), "j5");

  BOOST_REQUIRE_EQUAL(model.mimic_joint_supports.size(), 6);
  BOOST_CHECK((model.mimic_joint_supports[0] == Model::IndexVector({0})));
  BOOST_CHECK((model.mimic_joint_supports[1] == Model::IndexVector({0})));
  // Mimic joint should support itself
  BOOST_CHECK((model.mimic_joint_supports[2] == Model::IndexVector({0, 2})));
  // j3 is supported by j2
  BOOST_CHECK((model.mimic_joint_supports[3] == Model::IndexVector({0, 2})));
  BOOST_CHECK((model.mimic_joint_supports[4] == Model::IndexVector({0})));
  // Mimic joint should support itself
  BOOST_CHECK((model.mimic_joint_supports[5] == Model::IndexVector({0, 5})));
}

BOOST_AUTO_TEST_CASE(test_model_subspace_dimensions)
{
  Model model;
  buildModels::humanoidRandom(model);

  // Check that i ends supports[i]
  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
  {
    const Model::JointModel & jmodel = model.joints[joint_id];

    BOOST_CHECK(model.nqs[joint_id] == jmodel.nq());
    BOOST_CHECK(model.idx_qs[joint_id] == jmodel.idx_q());

    BOOST_CHECK(model.nvs[joint_id] == jmodel.nv());
    BOOST_CHECK(model.idx_vs[joint_id] == jmodel.idx_v());

    BOOST_CHECK(model.nvExtendeds[joint_id] == jmodel.nvExtended());
    BOOST_CHECK(model.idx_vExtendeds[joint_id] == jmodel.idx_vExtended());
  }
}

BOOST_AUTO_TEST_CASE(comparison)
{
  Model model;
  buildModels::humanoidRandom(model);

  BOOST_CHECK(model == model);
}

BOOST_AUTO_TEST_CASE(cast)
{
  Model model;
  buildModels::humanoidRandom(model);

  BOOST_CHECK(model.cast<double>() == model.cast<double>());
  BOOST_CHECK(model.cast<double>().cast<long double>() == model.cast<long double>());

  typedef ModelTpl<long double> Modelld;

  Modelld model2(model);
  BOOST_CHECK(model2 == model.cast<long double>());
}

// This is the minimal collection to build the `buildModels::manipulator` model
template<typename _Scalar, int _Options>
struct ManipulatorJointCollectionNoThrow
{
  typedef _Scalar Scalar;
  static constexpr int Options = _Options;

  // Joint Revolute
  typedef ::pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
  typedef ::pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;
  typedef ::pinocchio::JointModelRevoluteTpl<Scalar, Options, 2> JointModelRZ;

  typedef boost::variant<JointModelRX, JointModelRY, JointModelRZ> JointModelVariant;

  // Joint Revolute
  typedef ::pinocchio::JointDataRevoluteTpl<Scalar, Options, 0> JointDataRX;
  typedef ::pinocchio::JointDataRevoluteTpl<Scalar, Options, 1> JointDataRY;
  typedef ::pinocchio::JointDataRevoluteTpl<Scalar, Options, 2> JointDataRZ;

  typedef boost::variant<JointDataRX, JointDataRY, JointDataRZ> JointDataVariant;
};

template<typename _Scalar, int _Options>
struct ManipulatorJointCollectionThrow
{
  typedef _Scalar Scalar;
  static constexpr int Options = _Options;

  // Joint Revolute
  typedef ::pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
  typedef ::pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;

  typedef boost::variant<JointModelRX, JointModelRY> JointModelVariant;

  // Joint Revolute
  typedef ::pinocchio::JointDataRevoluteTpl<Scalar, Options, 0> JointDataRX;
  typedef ::pinocchio::JointDataRevoluteTpl<Scalar, Options, 1> JointDataRY;

  typedef boost::variant<JointDataRX, JointDataRY> JointDataVariant;
};

BOOST_AUTO_TEST_CASE(cast_collection)
{
  typedef ::pinocchio::ModelTpl<double, 0, JointCollectionDefaultTpl> Model;
  typedef ::pinocchio::ModelTpl<double, 0, ManipulatorJointCollectionNoThrow> MinimalModelNoThrow;
  typedef ::pinocchio::ModelTpl<double, 0, ManipulatorJointCollectionThrow> MinimalModelThrow;

  Model model_full;
  buildModels::manipulator(model_full);

  Model model_full_copy(model_full);
  BOOST_CHECK(model_full == model_full_copy);

  BOOST_CHECK_NO_THROW({ MinimalModelNoThrow model_no_throw = model_full; });

  BOOST_CHECK_THROW({ MinimalModelThrow model_throw = model_full; }, std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(test_std_vector_of_Model)
{
  Model model;
  buildModels::humanoid(model);

  std::vector<Model> models;
  for (size_t k = 0; k < 20; ++k)
  {
    models.push_back(Model());
    buildModels::humanoid(models.back());

    BOOST_CHECK(model == models.back());
  }
}

#ifdef PINOCCHIO_WITH_COLLISION
struct AddPrefix
{
  std::string p;
  std::string operator()(const std::string & n)
  {
    return p + n;
  }
  Frame operator()(const Frame & _f)
  {
    Frame f(_f);
    f.name = p + f.name;
    return f;
  }
  AddPrefix(const char * _p)
  : p(_p)
  {
  }
};

BOOST_AUTO_TEST_CASE(append)
{
  Model manipulator, humanoid;
  GeometryModel geomManipulator, geomHumanoid;

  buildModels::manipulator(manipulator, true);
  buildModels::manipulatorGeometries(manipulator, geomManipulator);
  geomManipulator.addAllCollisionPairs();
  // Add prefix to joint and frame names
  AddPrefix addManipulatorPrefix("manipulator/");
  std::transform(
    ++manipulator.names.begin(), manipulator.names.end(), ++manipulator.names.begin(),
    addManipulatorPrefix);
  std::transform(
    ++manipulator.frames.begin(), manipulator.frames.end(), ++manipulator.frames.begin(),
    addManipulatorPrefix);

  BOOST_TEST_MESSAGE(manipulator);

  buildModels::humanoid(humanoid);
  buildModels::humanoidGeometries(humanoid, geomHumanoid);
  geomHumanoid.addAllCollisionPairs();
  // Add prefix to joint and frame names
  AddPrefix addHumanoidPrefix("humanoid/");
  std::transform(
    ++humanoid.names.begin(), humanoid.names.end(), ++humanoid.names.begin(), addHumanoidPrefix);
  std::transform(
    ++humanoid.frames.begin(), humanoid.frames.end(), ++humanoid.frames.begin(), addHumanoidPrefix);

  BOOST_TEST_MESSAGE(humanoid);

  typename Model::ConfigVectorType humanoid_config_vector(humanoid.nq);
  typename Model::ConfigVectorType manipulator_config_vector(manipulator.nq);
  humanoid_config_vector = randomConfiguration(humanoid);
  manipulator_config_vector = randomConfiguration(manipulator);
  humanoid.referenceConfigurations.insert(std::make_pair("common_key", humanoid_config_vector));
  manipulator.referenceConfigurations.insert(
    std::make_pair("common_key", manipulator_config_vector));

  humanoid_config_vector = randomConfiguration(humanoid);
  manipulator_config_vector = randomConfiguration(manipulator);
  humanoid.referenceConfigurations.insert(std::make_pair("humanoid_key", humanoid_config_vector));
  manipulator.referenceConfigurations.insert(
    std::make_pair("manipulator_key", manipulator_config_vector));

  // TODO fix inertia of the base
  manipulator.inertias[0].setRandom();
  SE3 aMb = SE3::Random();

  // First append a model to the universe frame.
  Model model1;
  GeometryModel geomModel1;
  FrameIndex fid = 0;
  appendModel(
    humanoid, manipulator, geomHumanoid, geomManipulator, fid, SE3::Identity(), model1, geomModel1);
  typedef typename Model::ConfigVectorMap ConfigVectorMap;

  typename Model::ConfigVectorType neutral_config_vector(model1.nq);
  neutral(model1, neutral_config_vector);

  BOOST_CHECK(model1.referenceConfigurations.size() == 3);
  for (typename ConfigVectorMap::const_iterator config_it = model1.referenceConfigurations.begin();
       config_it != model1.referenceConfigurations.end(); ++config_it)
  {
    const std::string & config_name = config_it->first;
    const typename Model::ConfigVectorType & config_vector = config_it->second;

    typename ConfigVectorMap::const_iterator humanoid_config =
      humanoid.referenceConfigurations.find(config_name);
    typename ConfigVectorMap::const_iterator manipulator_config =
      manipulator.referenceConfigurations.find(config_name);
    for (JointIndex joint_id = 1; joint_id < model1.joints.size(); ++joint_id)
    {
      const JointModel & joint_model1 = model1.joints[joint_id];
      if (
        humanoid_config != humanoid.referenceConfigurations.end()
        && humanoid.existJointName(model1.names[joint_id]))
      { // key and joint exists in humanoid
        const JointModel & joint_model_humanoid =
          humanoid.joints[humanoid.getJointId(model1.names[joint_id])];
        BOOST_CHECK(
          joint_model_humanoid.jointConfigSelector(humanoid_config->second)
          == joint_model1.jointConfigSelector(config_vector));
        BOOST_CHECK(
          joint_model_humanoid.JointMappedConfigSelector(humanoid_config->second)
          == joint_model1.JointMappedConfigSelector(config_vector));
        // std::cerr<<"humanoid "<<config_name<<" "<<model1.names[joint_id]<<std::endl;
      }
      else if (
        manipulator_config != manipulator.referenceConfigurations.end()
        && manipulator.existJointName(model1.names[joint_id]))
      { // key and joint exists in manipulator.
        const JointModel & joint_model_manipulator =
          manipulator.joints[manipulator.getJointId(model1.names[joint_id])];
        BOOST_CHECK(
          joint_model_manipulator.jointConfigSelector(manipulator_config->second)
          == joint_model1.jointConfigSelector(config_vector));
        BOOST_CHECK(
          joint_model_manipulator.JointMappedConfigSelector(manipulator_config->second)
          == joint_model1.JointMappedConfigSelector(config_vector));
        // std::cerr<<"manipulator "<<config_name<<" "<<model1.names[joint_id]<<std::endl;
      }
      else
      { // joint and key combo not found, should with neutral
        BOOST_CHECK(
          joint_model1.jointConfigSelector(neutral_config_vector)
          == joint_model1.jointConfigSelector(config_vector));
        BOOST_CHECK(
          joint_model1.JointMappedConfigSelector(neutral_config_vector)
          == joint_model1.JointMappedConfigSelector(config_vector));
        // std::cerr<<"neutral "<<config_name<<" "<<model1.names[joint_id]<<std::endl;
      }
    }
  }

  {
    Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity());
    Model model3;
    appendModel(humanoid, manipulator, fid, SE3::Identity(), model3);
    BOOST_CHECK(model1 == model2);
    BOOST_CHECK(model1 == model3);
    BOOST_CHECK(model2 == model3);
  }

  Data data1(model1);
  BOOST_CHECK(model1.check(data1));

  BOOST_TEST_MESSAGE(model1);

  // Second, append a model to a moving frame.
  Model model;

  GeometryModel geomModel;
  fid = humanoid.addFrame(Frame(
    "humanoid/add_manipulator", humanoid.getJointId("humanoid/chest2_joint"),
    humanoid.getFrameId("humanoid/chest2_joint"), aMb, OP_FRAME));

  // Append manipulator to chest2_joint of humanoid
  appendModel(
    humanoid, manipulator, geomHumanoid, geomManipulator, fid, SE3::Identity(), model, geomModel);

  neutral_config_vector.resize(model.nq);
  neutral(model, neutral_config_vector);

  BOOST_CHECK(model.referenceConfigurations.size() == 3);
  for (typename ConfigVectorMap::const_iterator config_it = model.referenceConfigurations.begin();
       config_it != model.referenceConfigurations.end(); ++config_it)
  {
    const std::string & config_name = config_it->first;
    const typename Model::ConfigVectorType & config_vector = config_it->second;

    typename ConfigVectorMap::const_iterator humanoid_config =
      humanoid.referenceConfigurations.find(config_name);
    typename ConfigVectorMap::const_iterator manipulator_config =
      manipulator.referenceConfigurations.find(config_name);
    for (JointIndex joint_id = 1; joint_id < model.joints.size(); ++joint_id)
    {
      const JointModel & joint_model = model.joints[joint_id];
      if (
        humanoid_config != humanoid.referenceConfigurations.end()
        && humanoid.existJointName(model.names[joint_id]))
      { // key and joint exists in humanoid
        const JointModel & joint_model_humanoid =
          humanoid.joints[humanoid.getJointId(model.names[joint_id])];
        BOOST_CHECK(
          joint_model_humanoid.jointConfigSelector(humanoid_config->second)
          == joint_model.jointConfigSelector(config_vector));
        BOOST_CHECK(
          joint_model_humanoid.JointMappedConfigSelector(humanoid_config->second)
          == joint_model.JointMappedConfigSelector(config_vector));
        // std::cerr<<"humanoid "<<config_name<<" "<<model.names[joint_id]<<std::endl;
      }
      else if (
        manipulator_config != manipulator.referenceConfigurations.end()
        && manipulator.existJointName(model.names[joint_id]))
      { // key and joint exists in manipulator.
        const JointModel & joint_model_manipulator =
          manipulator.joints[manipulator.getJointId(model.names[joint_id])];
        BOOST_CHECK(
          joint_model_manipulator.jointConfigSelector(manipulator_config->second)
          == joint_model.jointConfigSelector(config_vector));
        BOOST_CHECK(
          joint_model_manipulator.JointMappedConfigSelector(manipulator_config->second)
          == joint_model.JointMappedConfigSelector(config_vector));
        // std::cerr<<"manipulator "<<config_name<<" "<<model.names[joint_id]<<std::endl;
      }
      else
      { // joint and key combo not found, should with neutral
        BOOST_CHECK(
          joint_model.jointConfigSelector(neutral_config_vector)
          == joint_model.jointConfigSelector(config_vector));
        BOOST_CHECK(
          joint_model.JointMappedConfigSelector(neutral_config_vector)
          == joint_model.JointMappedConfigSelector(config_vector));
        // std::cerr<<"neutral "<<config_name<<" "<<model.names[joint_id]<<std::endl;
      }
    }
  }

  {
    Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity());
    Model model3;
    appendModel(humanoid, manipulator, fid, SE3::Identity(), model3);
    BOOST_CHECK(model == model2);
    BOOST_CHECK(model == model3);
    BOOST_CHECK(model2 == model3);
  }

  BOOST_TEST_MESSAGE(model);

  Data data(model);
  BOOST_CHECK(model.check(data));

  // Check the model
  BOOST_CHECK_EQUAL(
    model.getJointId("humanoid/chest2_joint"),
    model.parents[model.getJointId("manipulator/shoulder1_joint")]);

  // check the joint order and the inertias
  // All the joints of the manipulator should be at the end of the merged model
  JointIndex hnj = (JointIndex)humanoid.njoints;
  JointIndex chest2 = model.getJointId("humanoid/chest2_joint");
  for (JointIndex jid = 1; jid < hnj; ++jid)
  {
    BOOST_TEST_MESSAGE("Checking joint " << jid << " " << model.names[jid]);
    BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]);
    if (jid != chest2)
      BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]);
    else
      BOOST_CHECK_MESSAGE(
        model.inertias[jid].isApprox(
          manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[jid]),
        model.inertias[jid] << " != "
                            << manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[jid]);

    BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]);
  }
  for (JointIndex jid = 1; jid < manipulator.joints.size() - 1; ++jid)
  {
    BOOST_TEST_MESSAGE("Checking joint " << hnj - 1 + jid << " " << model.names[hnj + jid]);
    BOOST_CHECK_EQUAL(model.names[hnj - 1 + jid], manipulator.names[jid]);
    BOOST_CHECK_EQUAL(model.inertias[hnj - 1 + jid], manipulator.inertias[jid]);
    if (jid == 1)
      BOOST_CHECK_EQUAL(
        model.jointPlacements[hnj - 1 + jid], aMb * manipulator.jointPlacements[jid]);
    else
      BOOST_CHECK_EQUAL(model.jointPlacements[hnj - 1 + jid], manipulator.jointPlacements[jid]);
  }
  // Check the frames
  for (FrameIndex fid = 1; fid < humanoid.frames.size(); ++fid)
  {
    const Frame &frame = humanoid.frames[fid], parent = humanoid.frames[frame.parentFrame];
    BOOST_CHECK(model.existFrame(frame.name, frame.type));
    const Frame &nframe = model.frames[model.getFrameId(frame.name, frame.type)],
                nparent = model.frames[nframe.parentFrame];
    BOOST_CHECK_EQUAL(parent.name, nparent.name);
    BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
  }
  for (FrameIndex fid = 1; fid < manipulator.frames.size(); ++fid)
  {
    const Frame &frame = manipulator.frames[fid], parent = manipulator.frames[frame.parentFrame];
    BOOST_CHECK(model.existFrame(frame.name, frame.type));
    const Frame &nframe = model.frames[model.getFrameId(frame.name, frame.type)],
                nparent = model.frames[nframe.parentFrame];
    if (frame.parentFrame > 0)
    {
      BOOST_CHECK_EQUAL(parent.name, nparent.name);
      BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
    }
  }

  {
    Inertia inertia(2., Eigen::Vector3d(0.1, 0.1, 0.1), Eigen::Matrix3d::Identity());
    Frame additional_frame("inertial_frame", 2, SE3::Identity(), FrameType::JOINT, inertia);
    humanoid.addFrame(additional_frame);
    double mass_humanoid = computeTotalMass(humanoid);
    double mass_manipulator = computeTotalMass(manipulator);
    double total_mass = mass_manipulator + mass_humanoid;

    Model model4;
    GeometryModel geomModel4;
    appendModel(
      humanoid, manipulator, geomHumanoid, geomManipulator, 0, SE3::Identity(), model4, geomModel4);
    BOOST_CHECK_CLOSE(computeTotalMass(model4), total_mass, 1e-6);
  }
  {
    Model ff_model;
    auto ff_id = ff_model.addJoint(0, JointModelFreeFlyer(), SE3::Identity(), "floating_base");
    ff_model.addJointFrame(ff_id);
    GeometryModel ff_geom_model = GeometryModel();
    FrameIndex frame_id = ff_model.getFrameId("floating_base");
    Model model4;
    GeometryModel geomModel4;
    appendModel(
      ff_model, manipulator, ff_geom_model, geomManipulator, frame_id, SE3::Identity(), model4,
      geomModel4);
    BOOST_CHECK(model4.inertias[1] == model4.inertias[1]);
  }

  {
    Model model5, gripperModel;

    Inertia inertia(1., SE3::Vector3(0.5, 0., 0.0), SE3::Matrix3::Identity());
    SE3 pos(1);

    pos.translation() = SE3::LinearType(0.1, 0., 0.);
    JointIndex idx = gripperModel.addJoint(0, JointModelPX(), pos, "left_finger");
    gripperModel.addJointFrame(idx);

    pos.translation() = SE3::LinearType(-0.1, 0., 0.);
    idx = gripperModel.addJoint(0, JointModelPX(), pos, "right_finger");
    gripperModel.addJointFrame(idx);

    SE3 transformManGr = SE3::Random();
    FrameIndex fid = (FrameIndex)(manipulator.frames.size() - 1);
    appendModel(manipulator, gripperModel, fid, transformManGr, model5);

    JointIndex jid5 = model5.getJointId("left_finger");
    JointIndex jidG = gripperModel.getJointId("left_finger");
    BOOST_CHECK(
      model5.jointPlacements[jid5].isApprox(transformManGr * gripperModel.jointPlacements[jidG]));

    jid5 = model5.getJointId("right_finger");
    jidG = gripperModel.getJointId("right_finger");
    BOOST_CHECK(
      model5.jointPlacements[jid5].isApprox(transformManGr * gripperModel.jointPlacements[jidG]));
  }
}
#endif

BOOST_AUTO_TEST_CASE(test_buildReducedModel_empty)
{
  Model humanoid_model;
  buildModels::humanoid(humanoid_model);

  static const std::vector<JointIndex> empty_index_vector;

  humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
  humanoid_model.upperPositionLimit.head<3>().fill(1.);

  humanoid_model.referenceConfigurations.insert(
    std::pair<std::string, Eigen::VectorXd>("neutral", neutral(humanoid_model)));
  Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);
  Model humanoid_copy_model =
    buildReducedModel(humanoid_model, empty_index_vector, reference_config_humanoid);

  BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names);
  BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints);
  BOOST_CHECK(humanoid_copy_model == humanoid_model);
  BOOST_CHECK(
    humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model)));

  const std::vector<JointIndex> empty_joints_to_lock;

  const Model reduced_humanoid_model =
    buildReducedModel(humanoid_model, empty_joints_to_lock, reference_config_humanoid);
  BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints);
  BOOST_CHECK(reduced_humanoid_model.frames == humanoid_model.frames);
  BOOST_CHECK(reduced_humanoid_model.jointPlacements == humanoid_model.jointPlacements);
  BOOST_CHECK(reduced_humanoid_model.joints == humanoid_model.joints);

  for (JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
  {
    BOOST_CHECK(
      reduced_humanoid_model.inertias[joint_id].isApprox(humanoid_model.inertias[joint_id]));
  }
}

BOOST_AUTO_TEST_CASE(test_buildReducedModel)
{
  Model humanoid_model;
  buildModels::humanoid(humanoid_model);

  static const std::vector<JointIndex> empty_index_vector;

  humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
  humanoid_model.upperPositionLimit.head<3>().fill(1.);
  humanoid_model.armature.setOnes();
  humanoid_model.rotorGearRatio.setOnes();
  humanoid_model.rotorInertia.setOnes();

  humanoid_model.referenceConfigurations.insert(
    std::pair<std::string, Eigen::VectorXd>("neutral", neutral(humanoid_model)));
  Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);
  Model humanoid_copy_model =
    buildReducedModel(humanoid_model, empty_index_vector, reference_config_humanoid);

  BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names);
  BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints);
  BOOST_CHECK(humanoid_copy_model == humanoid_model);
  BOOST_CHECK(
    humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model)));

  std::vector<JointIndex> joints_to_lock;
  const std::string joint1_to_lock = "rarm_shoulder2_joint";
  joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock));
  const std::string joint2_to_lock = "larm_shoulder2_joint";
  joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock));

  Model reduced_humanoid_model =
    buildReducedModel(humanoid_model, joints_to_lock, reference_config_humanoid);
  BOOST_CHECK(
    reduced_humanoid_model.njoints == humanoid_model.njoints - (int)joints_to_lock.size());

  BOOST_CHECK(reduced_humanoid_model != humanoid_model);

  Model reduced_humanoid_model_other_signature;
  buildReducedModel(
    humanoid_model, joints_to_lock, reference_config_humanoid,
    reduced_humanoid_model_other_signature);
  BOOST_CHECK(reduced_humanoid_model == reduced_humanoid_model_other_signature);

  // Check that forward kinematics give same results
  Data data(humanoid_model), reduced_data(reduced_humanoid_model);
  Eigen::VectorXd q = reference_config_humanoid;
  Eigen::VectorXd reduced_q(reduced_humanoid_model.nq);

  for (JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
  {
    const JointIndex reference_joint_id =
      humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]);

    reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) =
      humanoid_model.joints[reference_joint_id].jointConfigSelector(q);
    reduced_humanoid_model.joints[joint_id].JointMappedConfigSelector(reduced_q) =
      humanoid_model.joints[reference_joint_id].JointMappedConfigSelector(q);
  }

  BOOST_CHECK(reduced_humanoid_model.referenceConfigurations["neutral"].isApprox(
    neutral(reduced_humanoid_model)));

  framesForwardKinematics(humanoid_model, data, q);
  framesForwardKinematics(reduced_humanoid_model, reduced_data, reduced_q);

  for (size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id)
  {
    const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id];
    switch (reduced_frame.type)
    {
    case JOINT:
    case FIXED_JOINT: {
      // May not be present in the original model
      if (humanoid_model.existJointName(reduced_frame.name))
      {
        const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name);
        BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id]));
      }
      else if (humanoid_model.existFrame(reduced_frame.name))
      {
        const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
        BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
      }
      else
      {
        BOOST_CHECK_MESSAGE(
          false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model");
      }
      break;
    }
    default: {
      BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name));
      const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
      BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
      break;
    }
    }
  }

  // Check other model fields values
  for (JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
  {
    const JointIndex initial_joint_id =
      humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]);

    const JointModel & reduced_joint = reduced_humanoid_model.joints[joint_id];
    const JointModel & initial_joint = humanoid_model.joints[initial_joint_id];
#define CHECK_FIELD_VALUE(field)                                                                   \
  BOOST_CHECK(                                                                                     \
    reduced_joint.jointVelocitySelector(reduced_humanoid_model.field)                              \
    == initial_joint.jointVelocitySelector(humanoid_model.field))

    CHECK_FIELD_VALUE(armature);
    CHECK_FIELD_VALUE(rotorGearRatio);
    CHECK_FIELD_VALUE(rotorInertia);

#undef CHECK_FIELD_VALUE
  }
}

#ifdef PINOCCHIO_WITH_COLLISION
BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom)
{
  Model humanoid_model;
  buildModels::humanoid(humanoid_model);

  humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
  humanoid_model.upperPositionLimit.head<3>().fill(1.);
  const Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);

  GeometryModel humanoid_geometry;
  buildModels::humanoidGeometries(humanoid_model, humanoid_geometry);

  static const std::vector<JointIndex> empty_index_vector;

  Model humanoid_copy_model;
  GeometryModel humanoid_copy_geometry;
  buildReducedModel(
    humanoid_model, humanoid_geometry, empty_index_vector, reference_config_humanoid,
    humanoid_copy_model, humanoid_copy_geometry);

  BOOST_CHECK(humanoid_copy_model == humanoid_model);
  BOOST_CHECK(humanoid_copy_geometry == humanoid_geometry);

  std::vector<JointIndex> joints_to_lock;
  const std::string joint1_to_lock = "rarm_shoulder2_joint";
  joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock));
  const std::string joint2_to_lock = "larm_shoulder2_joint";
  joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock));

  Model reduced_humanoid_model;
  GeometryModel reduced_humanoid_geometry;
  buildReducedModel(
    humanoid_model, humanoid_geometry, joints_to_lock, reference_config_humanoid,
    reduced_humanoid_model, reduced_humanoid_geometry);

  BOOST_CHECK(reduced_humanoid_geometry.ngeoms == humanoid_geometry.ngeoms);
  BOOST_CHECK(reduced_humanoid_geometry.collisionPairs == humanoid_geometry.collisionPairs);
  BOOST_CHECK(
    reduced_humanoid_geometry.geometryObjects.size() == humanoid_geometry.geometryObjects.size());

  for (Index i = 0; i < humanoid_geometry.geometryObjects.size(); ++i)
  {
    const GeometryObject & go1 = humanoid_geometry.geometryObjects[i];
    const GeometryObject & go2 = reduced_humanoid_geometry.geometryObjects[i];
    BOOST_CHECK_EQUAL(go1.name, go2.name);
    BOOST_CHECK_EQUAL(go1.geometry, go2.geometry);
    BOOST_CHECK_EQUAL(go1.meshPath, go2.meshPath);
    BOOST_CHECK_EQUAL(go1.meshScale, go2.meshScale);
    BOOST_CHECK_EQUAL(go1.overrideMaterial, go2.overrideMaterial);
    BOOST_CHECK_EQUAL(go1.meshColor, go2.meshColor);
    BOOST_CHECK_EQUAL(go1.meshTexturePath, go2.meshTexturePath);
    BOOST_CHECK_EQUAL(go1.parentFrame, go2.parentFrame);
    BOOST_CHECK_EQUAL(
      humanoid_model.frames[go1.parentFrame].name,
      reduced_humanoid_model.frames[go2.parentFrame].name);
  }

  Data data(humanoid_model), reduced_data(reduced_humanoid_model);
  const Eigen::VectorXd q = reference_config_humanoid;
  Eigen::VectorXd reduced_q(reduced_humanoid_model.nq);

  for (JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
  {
    const JointIndex reference_joint_id =
      humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]);

    reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) =
      humanoid_model.joints[reference_joint_id].jointConfigSelector(q);
    reduced_humanoid_model.joints[joint_id].JointMappedConfigSelector(reduced_q) =
      humanoid_model.joints[reference_joint_id].JointMappedConfigSelector(q);
  }

  framesForwardKinematics(humanoid_model, data, q);
  framesForwardKinematics(reduced_humanoid_model, reduced_data, reduced_q);

  for (size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id)
  {
    const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id];
    switch (reduced_frame.type)
    {
    case JOINT:
    case FIXED_JOINT: {
      // May not be present in the original model
      if (humanoid_model.existJointName(reduced_frame.name))
      {
        const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name);
        BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id]));
      }
      else if (humanoid_model.existFrame(reduced_frame.name))
      {
        const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
        BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
      }
      else
      {
        BOOST_CHECK_MESSAGE(
          false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model");
      }
      break;
    }
    default: {
      BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name));
      const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
      BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
      break;
    }
    }
  }

  // Test GeometryObject placements
  GeometryData geom_data(humanoid_geometry), reduded_geom_data(reduced_humanoid_geometry);
  updateGeometryPlacements(humanoid_model, data, humanoid_geometry, geom_data);
  updateGeometryPlacements(
    reduced_humanoid_model, reduced_data, reduced_humanoid_geometry, reduded_geom_data);

  BOOST_CHECK(geom_data.oMg.size() == reduded_geom_data.oMg.size());
  for (FrameIndex i = 0; i < geom_data.oMg.size(); ++i)
  {
    BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i]));
  }

  // Test other signature
  std::vector<GeometryModel> full_geometry_models;
  full_geometry_models.push_back(humanoid_geometry);
  full_geometry_models.push_back(humanoid_geometry);
  full_geometry_models.push_back(humanoid_geometry);

  std::vector<GeometryModel> reduced_geometry_models;

  Model reduced_humanoid_model_other_sig;
  buildReducedModel(
    humanoid_model, full_geometry_models, joints_to_lock, reference_config_humanoid,
    reduced_humanoid_model_other_sig, reduced_geometry_models);

  BOOST_CHECK(reduced_geometry_models[0] == reduced_humanoid_geometry);
  BOOST_CHECK(reduced_geometry_models[1] == reduced_humanoid_geometry);
  BOOST_CHECK(reduced_geometry_models[2] == reduced_humanoid_geometry);
}
#endif // PINOCCHIO_WITH_COLLISION

BOOST_AUTO_TEST_CASE(test_findCommonAncestor)
{
  Model model;
  buildModels::humanoid(model);

  {
    size_t id_ancestor1, id_ancestor2;
    JointIndex ancestor = findCommonAncestor(model, 0, 0, id_ancestor1, id_ancestor2);
    BOOST_CHECK(ancestor == 0);
    BOOST_CHECK(id_ancestor1 == 0);
    BOOST_CHECK(id_ancestor2 == 0);
  }

  {
    size_t id_ancestor1, id_ancestor2;
    JointIndex ancestor =
      findCommonAncestor(model, 0, (JointIndex)(model.njoints - 1), id_ancestor1, id_ancestor2);
    BOOST_CHECK(ancestor == 0);
    BOOST_CHECK(id_ancestor1 == 0);
    BOOST_CHECK(id_ancestor2 == 0);
  }

  {
    size_t id_ancestor1, id_ancestor2;
    JointIndex ancestor =
      findCommonAncestor(model, (JointIndex)(model.njoints - 1), 0, id_ancestor1, id_ancestor2);
    BOOST_CHECK(ancestor == 0);
    BOOST_CHECK(id_ancestor1 == 0);
    BOOST_CHECK(id_ancestor2 == 0);
  }

  {
    size_t id_ancestor1, id_ancestor2;
    JointIndex ancestor =
      findCommonAncestor(model, (JointIndex)(model.njoints - 1), 1, id_ancestor1, id_ancestor2);
    BOOST_CHECK(ancestor == 1);
    BOOST_CHECK(model.supports[(JointIndex)(model.njoints - 1)][id_ancestor1] == ancestor);
    BOOST_CHECK(model.supports[1][id_ancestor2] == ancestor);
  }
}

BOOST_AUTO_TEST_CASE(test_has_configuration_limit)
{
  using namespace Eigen;

  // Test joint specific function hasConfigurationLimit
  JointModelFreeFlyer test_joint_ff = JointModelFreeFlyer();
  std::vector<bool> cf_limits_ff = test_joint_ff.hasConfigurationLimit();
  std::vector<bool> cf_limits_tangent_ff = test_joint_ff.hasConfigurationLimitInTangent();
  std::vector<bool> expected_cf_limits_ff({true, true, true, false, false, false, false});
  std::vector<bool> expected_cf_limits_tangent_ff({true, true, true, false, false, false});
  BOOST_CHECK(cf_limits_ff == expected_cf_limits_ff);
  BOOST_CHECK(cf_limits_tangent_ff == expected_cf_limits_tangent_ff);

  JointModelPlanar test_joint_planar = JointModelPlanar();
  std::vector<bool> cf_limits_planar = test_joint_planar.hasConfigurationLimit();
  std::vector<bool> cf_limits_tangent_planar = test_joint_planar.hasConfigurationLimitInTangent();
  std::vector<bool> expected_cf_limits_planar({true, true, false, false});
  std::vector<bool> expected_cf_limits_tangent_planar({true, true, false});
  BOOST_CHECK(cf_limits_planar == expected_cf_limits_planar);
  BOOST_CHECK(cf_limits_tangent_planar == expected_cf_limits_tangent_planar);

  JointModelPX test_joint_p = JointModelPX();
  std::vector<bool> cf_limits_prismatic = test_joint_p.hasConfigurationLimit();
  std::vector<bool> cf_limits_tangent_prismatic = test_joint_p.hasConfigurationLimitInTangent();
  std::vector<bool> expected_cf_limits_prismatic({true});
  std::vector<bool> expected_cf_limits_tangent_prismatic({true});
  BOOST_CHECK(cf_limits_prismatic == expected_cf_limits_prismatic);
  BOOST_CHECK(cf_limits_tangent_prismatic == expected_cf_limits_tangent_prismatic);

  // Test model.hasConfigurationLimit() function
  Model model;
  JointIndex jointId = 0;

  jointId = model.addJoint(jointId, JointModelFreeFlyer(), SE3::Identity(), "Joint0");
  jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
  jointId = model.addJoint(
    jointId, JointModelRUBZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");

  std::vector<bool> expected_cf_limits_model(
    {true, true, true,           // translation of FF
     false, false, false, false, // rotation of FF
     true,                       // roational joint
     false, false});             // unbounded rotational joint
  std::vector<bool> model_cf_limits = model.hasConfigurationLimit();
  BOOST_CHECK((model_cf_limits == expected_cf_limits_model));

  // Test model.hasConfigurationLimitInTangent() function
  std::vector<bool> expected_cf_limits_tangent_model(
    {true, true, true,    // translation of FF
     false, false, false, // rotation of FF
     true,                // roational joint
     false});             // unbounded rotational joint
  std::vector<bool> model_cf_limits_tangent = model.hasConfigurationLimitInTangent();
  BOOST_CHECK((model_cf_limits_tangent == expected_cf_limits_tangent_model));
}

/// Test transformJointIntoMimic by creating a model manually and
/// a model modified with transformJointIntoMimic.
BOOST_AUTO_TEST_CASE(test_transform_to_mimic)
{
  Model model_exp;
  // j1 -- j2 -- j3
  //    -- j4 -- j5
  // with j5 mimicking j3
  // with j2 mimicking j1
  model_exp.addJoint(0, JointModelRX(), SE3::Identity(), "j1");
  model_exp.addJoint(
    1, JointModelMimic(JointModelRX(), model_exp.joints[1], 1., 0.), SE3::Identity(), "j2");
  model_exp.addJoint(2, JointModelRX(), SE3::Identity(), "j3");
  model_exp.addJoint(1, JointModelRX(), SE3::Identity(), "j4");
  model_exp.addJoint(
    4, JointModelMimic(JointModelRX(), model_exp.joints[3], 1., 0.), SE3::Identity(), "j5");

  // Model transformed with transformJointIntoMimic
  Model model;
  Model model_mimic;
  Model model_mimic2;
  model.addJoint(0, JointModelRX(), SE3::Identity(), "j1");
  model.addJoint(1, JointModelRX(), SE3::Identity(), "j2");
  model.addJoint(2, JointModelRX(), SE3::Identity(), "j3");
  model.addJoint(1, JointModelRX(), SE3::Identity(), "j4");
  model.addJoint(4, JointModelRX(), SE3::Identity(), "j5");
  transformJointIntoMimic(model, 3, 5, 1., 0., model_mimic);
  transformJointIntoMimic(model_mimic, 1, 2, 1., 0., model_mimic2);

  BOOST_CHECK(model_mimic2 == model_exp);
}

/// Test buildMimicModel by creating a model manually and
/// a model modified with buildMimicModel
BOOST_AUTO_TEST_CASE(test_build_mimic_model)
{
  Model model_exp;
  // j1 -- j2 -- j3
  //    -- j4 -- j5
  // with j5 mimicking j3
  // with j2 mimicking j1
  model_exp.addJoint(0, JointModelRX(), SE3::Identity(), "j1");
  model_exp.addJoint(
    1, JointModelMimic(JointModelRX(), model_exp.joints[1], 1., 0.), SE3::Identity(), "j2");
  model_exp.addJoint(2, JointModelRX(), SE3::Identity(), "j3");
  model_exp.addJoint(1, JointModelRX(), SE3::Identity(), "j4");
  model_exp.addJoint(
    4, JointModelMimic(JointModelRX(), model_exp.joints[3], 2., 1.), SE3::Identity(), "j5");

  // Model transformed with buildMimicModel
  Model model;
  Model model_mimic;
  model.addJoint(0, JointModelRX(), SE3::Identity(), "j1");
  model.addJoint(1, JointModelRX(), SE3::Identity(), "j2");
  model.addJoint(2, JointModelRX(), SE3::Identity(), "j3");
  model.addJoint(1, JointModelRX(), SE3::Identity(), "j4");
  model.addJoint(4, JointModelRX(), SE3::Identity(), "j5");
  buildMimicModel(model, {1, 3}, {2, 5}, {1., 2.}, {0., 1.}, model_mimic);

  BOOST_CHECK(model_mimic == model_exp);
}

BOOST_AUTO_TEST_CASE(test_cast_mimic)
{
  Model humanoid_model, humanoid_mimic;
  buildModels::humanoid(humanoid_model);
  Data data(humanoid_model);
  BOOST_CHECK(humanoid_model.check(data));

  JointIndex index_p = humanoid_model.getJointId("rleg_shoulder3_joint");
  JointIndex index_s = humanoid_model.getJointId("lleg_shoulder3_joint");

  transformJointIntoMimic(humanoid_model, index_p, index_s, 2.0, 0.4, humanoid_mimic);
  data = Data(humanoid_mimic);
  BOOST_CHECK(humanoid_mimic.check(data));

  humanoid_mimic = humanoid_mimic.cast<double>();
  data = Data(humanoid_mimic);
  BOOST_CHECK(humanoid_mimic.check(data));
  index_s = humanoid_mimic.getJointId("lleg_shoulder3_joint");
  BOOST_CHECK_EQUAL(humanoid_mimic.idx_qs[index_s], humanoid_mimic.joints[index_s].idx_q());
  BOOST_CHECK_EQUAL(humanoid_mimic.idx_vs[index_s], humanoid_mimic.joints[index_s].idx_v());

  ModelTpl<float> humanoid_mimic_f = humanoid_mimic.cast<float>();
  DataTpl<float> data_f(humanoid_mimic_f);
  BOOST_CHECK(humanoid_mimic_f.check(data_f));
  index_s = humanoid_mimic_f.getJointId("lleg_shoulder3_joint");
  BOOST_CHECK_EQUAL(humanoid_mimic_f.idx_qs[index_s], humanoid_mimic_f.joints[index_s].idx_q());
  BOOST_CHECK_EQUAL(humanoid_mimic_f.idx_vs[index_s], humanoid_mimic_f.joints[index_s].idx_v());
}

BOOST_AUTO_TEST_CASE(test_build_reduced_model_mimic)
{
  typedef Model::JointCollection JC;
  typedef Model::JointIndex JointIndex;
  typedef JC::JointModelRX JointModelRX;
  typedef JC::JointModelRUBX JointModelRUBX;
  Model model;

  JointIndex ffidx = model.addJoint(0, JC::JointModelFreeFlyer(), SE3::Identity(), "root_joint");
  model.addJointFrame(ffidx);
  JointIndex idx = model.addJoint(ffidx, JointModelRX(), SE3::Identity(), "joint1");
  model.addJointFrame(idx);
  idx = model.addJoint(idx, JointModelRUBX(), SE3::Identity(), "joint2");
  model.addJointFrame(idx);
  idx = model.addJoint(idx, JointModelRX(), SE3::Identity(), "joint3");
  model.addJointFrame(idx);

  auto const mimic = JointModelMimic(
    boost::get<JointModelRX>(model.joints[model.getJointId("joint3")].toVariant()), 1., 0.);
  idx = model.addJoint(idx, mimic, SE3::Identity(), "joint4");
  model.addJointFrame(idx);

  // We lock a 2 DoF joint that is before the mimicked joint.
  const std::vector<JointIndex> joints_to_lock = {model.getJointId("joint2")};
  Model reduced_model;
  BOOST_CHECK_NO_THROW(buildReducedModel(model, joints_to_lock, neutral(model), reduced_model));
}

BOOST_AUTO_TEST_CASE(test_mimicked_mimicking_vectors)
{
  Model man_model, man_mimic;
  buildModels::manipulator(man_model);

  // No mimic in model, so vectors should be empty
  BOOST_CHECK(man_model.mimicked_joints.size() == 0);
  BOOST_CHECK(man_model.mimicking_joints.size() == 0);

  buildMimicModel(man_model, {1, 3}, {2, 5}, {1., 2.}, {0., 1.}, man_mimic);

  // Vectors should be of size 2
  BOOST_CHECK(man_mimic.mimicked_joints.size() == 2);
  BOOST_CHECK(man_mimic.mimicking_joints.size() == 2);
  for (size_t i = 1; i < man_mimic.mimicked_joints.size(); i++)
  {
    BOOST_CHECK(man_mimic.mimicked_joints[i - 1] < man_mimic.mimicked_joints[i]);
    BOOST_CHECK(man_mimic.mimicking_joints[i - 1] < man_mimic.mimicking_joints[i]);
  }
}

BOOST_AUTO_TEST_CASE(test_has_configuration_limit_mimic)
{
  Model model;
  // j1 -- j2
  //    -- j3
  // with j3 mimicking j2
  model.addJoint(0, JointModelRX(), SE3::Identity(), "j1");
  model.addJoint(1, JointModelRX(), SE3::Identity(), "j2");
  model.addJoint(
    2, JointModelMimic(JointModelRX(), model.joints[1], 1., 0.), SE3::Identity(), "j3");

  BOOST_CHECK_EQUAL(model.lowerPositionLimit.size(), 2);
  BOOST_CHECK_EQUAL(model.upperPositionLimit.size(), 2);
  BOOST_CHECK_EQUAL(model.hasConfigurationLimit().size(), 2);
  BOOST_CHECK_EQUAL(model.hasConfigurationLimitInTangent().size(), 2);
}

// Non regression test for: https://github.com/stack-of-tasks/pinocchio/issues/2805
// When a frame with non null placement have inertia, the inertia was not well transformed
// in the support joint.
BOOST_AUTO_TEST_CASE(test_append_model_issue_2805)
{
  // Create robot with a fixed joint
  Model model_fixed_joint;
  Inertia I(1000, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity() * 166);
  SE3 translation(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitX());
  auto root_joint_index = model_fixed_joint.addJoint(
    0, pinocchio::JointModel(pinocchio::JointModelFreeFlyer()), SE3::Identity(), "root_joint");
  auto root_joint_frame_index =
    model_fixed_joint.addFrame(Frame("root_joint", root_joint_index, 0, SE3::Identity(), JOINT));
  auto link_1_index = model_fixed_joint.addFrame(
    Frame("link_1", root_joint_index, root_joint_frame_index, SE3::Identity(), BODY, I));
  auto joint1_fixed_index = model_fixed_joint.addFrame(
    Frame("urdf_joint_1fixed", root_joint_index, link_1_index, translation, FIXED_JOINT));
  model_fixed_joint.addFrame(
    Frame("link_2", root_joint_index, joint1_fixed_index, translation, BODY, I));

  Model model_root;
  auto new_model = appendModel(model_root, model_fixed_joint, 0, SE3::Identity());

  BOOST_CHECK(new_model.inertias[1].isApprox(model_fixed_joint.inertias[1]));
}

// Non regression test for: https://github.com/stack-of-tasks/pinocchio/issues/2805
// Same test as above but verify the special case of frame attached to universe frame.
BOOST_AUTO_TEST_CASE(test_append_model_universe_with_inertia_issue_2805)
{
  Inertia I(1000, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity() * 166);
  SE3 translation_root_joint(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitX());
  SE3 translation_fixed_link_1(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitY());
  SE3 translation_fixed_link_2(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.));
  SE3 translation_AB(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitZ());

  // Base model
  Model model_a;
  auto model_a_root_joint_index = model_a.addJoint(
    0, pinocchio::JointModel(pinocchio::JointModelFreeFlyer()), SE3::Identity(), "root_joint");
  auto model_a_root_joint_frame = model_a.addFrame(
    Frame("root_joint", model_a_root_joint_index, 0, translation_root_joint, JOINT));

  //  Model with frame and inertia attached to universe.
  //  To reproduce the issue, we must have two frame with inertia attached to universe.
  //  With only one frame, some computation will give a 0 mass inertia that will hide the bug.
  Model model_b;
  model_b.addFrame(Frame("fixed_link_1", 0, 0, translation_fixed_link_1, BODY, I));
  model_b.addFrame(Frame("fixed_link_2", 0, 0, translation_fixed_link_2, BODY, I));

  Model expected_model;
  auto expected_model_root_joint_index = expected_model.addJoint(
    0, pinocchio::JointModel(pinocchio::JointModelFreeFlyer()), SE3::Identity(), "root_joint");
  auto expected_model_root_joint_frame_index = expected_model.addFrame(
    Frame("root_joint", expected_model_root_joint_index, 0, translation_root_joint, JOINT));
  auto fixed_link_1_frame_index = expected_model.addFrame(Frame(
    "fixed_link_1", expected_model_root_joint_index, expected_model_root_joint_frame_index,
    translation_root_joint.act(translation_AB.act(translation_fixed_link_1)), BODY, I));
  auto fixed_link_2_frame_index = expected_model.addFrame(Frame(
    "fixed_link_2", expected_model_root_joint_index, expected_model_root_joint_frame_index,
    translation_root_joint.act(translation_AB.act(translation_fixed_link_2)), BODY, I));

  // Test frame position and inertias attached to root_joint
  auto new_model = appendModel(model_a, model_b, model_a_root_joint_frame, translation_AB);
  BOOST_CHECK(new_model.frames[fixed_link_1_frame_index].placement.isApprox(
    expected_model.frames[fixed_link_1_frame_index].placement));
  BOOST_CHECK(new_model.frames[fixed_link_2_frame_index].placement.isApprox(
    expected_model.frames[fixed_link_2_frame_index].placement));
  BOOST_CHECK(new_model.inertias[1].isApprox(expected_model.inertias[1]));
}

BOOST_AUTO_TEST_CASE(test_colwise_sparsity_pattern_and_span_indexes)
{
  // --- Exact value check on a simple sequential 1-DOF chain ---
  Model chain;
  chain.addJoint(0, JointModelRX(), SE3::Identity(), "j1");
  chain.addJoint(1, JointModelRY(), SE3::Identity(), "j2");
  chain.addJoint(2, JointModelRZ(), SE3::Identity(), "j3");

  // sparsity_pattern_vector is indexed by joint_id:
  //   index 0 = universe (nv=0, all-false pattern)
  //   index 1 = j1, index 2 = j2, index 3 = j3
  BOOST_REQUIRE_EQUAL(chain.sparsity_pattern_vector.size(), 4u);
  BOOST_REQUIRE_EQUAL(chain.span_indexes_vector.size(), 4u);

  // All patterns must have the final nv as size
  for (const auto & pat : chain.sparsity_pattern_vector)
    BOOST_CHECK_EQUAL(pat.size(), chain.nv);

  // j1 (col 0): only its own DOF — joint_id = 1
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[1].size(), 1u);
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[1][0], 0);
  BOOST_CHECK(chain.sparsity_pattern_vector[1][0]);
  BOOST_CHECK(!chain.sparsity_pattern_vector[1][1]);
  BOOST_CHECK(!chain.sparsity_pattern_vector[1][2]);

  // j2 (col 1): j1's DOF + own DOF — joint_id = 2
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[2].size(), 2u);
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[2][0], 0);
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[2][1], 1);
  BOOST_CHECK(chain.sparsity_pattern_vector[2][0]);
  BOOST_CHECK(chain.sparsity_pattern_vector[2][1]);
  BOOST_CHECK(!chain.sparsity_pattern_vector[2][2]);

  // j3 (col 2): j1's + j2's + own DOF — joint_id = 3
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[3].size(), 3u);
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[3][0], 0);
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[3][1], 1);
  BOOST_CHECK_EQUAL(chain.span_indexes_vector[3][2], 2);
  BOOST_CHECK(chain.sparsity_pattern_vector[3][0]);
  BOOST_CHECK(chain.sparsity_pattern_vector[3][1]);
  BOOST_CHECK(chain.sparsity_pattern_vector[3][2]);

  // --- Consistency check on humanoidRandom ---
  Model model;
  buildModels::humanoidRandom(model);

  // One entry per joint (indexed by joint_id, including universe at index 0)
  BOOST_CHECK_EQUAL(model.sparsity_pattern_vector.size(), (size_t)model.njoints);
  BOOST_CHECK_EQUAL(model.span_indexes_vector.size(), (size_t)model.njoints);

  for (size_t i = 0; i < model.sparsity_pattern_vector.size(); ++i)
  {
    const auto & pattern = model.sparsity_pattern_vector[i];
    const auto & span_idx = model.span_indexes_vector[i];

    // Patterns must be resized to the final nv
    BOOST_CHECK_EQUAL(pattern.size(), model.nv);

    // All span indexes must be in [0, nv)
    for (const auto idx : span_idx)
    {
      BOOST_CHECK_GE(idx, 0);
      BOOST_CHECK_LT(idx, model.nv);
    }

    // Cross-check: number of true entries in pattern == size of span_indexes
    const Eigen::Index n_true = pattern.count();
    BOOST_CHECK_EQUAL(n_true, (Eigen::Index)span_idx.size());

    // Every span index maps to a true entry in the pattern
    for (const auto idx : span_idx)
      BOOST_CHECK(pattern[idx]);
  }
}

BOOST_AUTO_TEST_SUITE_END()
