//

// Copyright (c) 2019-2020 INRIA

//

#include "pinocchio/bindings/python/algorithm/algorithms.hpp"

#include "pinocchio/bindings/python/utils/list.hpp"

#include "pinocchio/bindings/python/utils/std-vector.hpp"

#include "pinocchio/algorithm/model.hpp"

namespace pinocchio

{

  namespace python

  {

    namespace bp = boost::python;

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>

                                const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,

                                const GeometryModel & geomModelA,

                                const GeometryModel & geomModelB,

                                const FrameIndex frameInModelA,

                                const SE3Tpl<Scalar,Options> & aMb)

    {

      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;

    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>

    bp::tuple

                      const GeometryModel & geom_model,

                      const std::vector<JointIndex> & list_of_joints_to_lock,

                      const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)

    {

      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;

                        reference_configuration,reduced_model,reduced_geom_model);

    }

    template <typename Scalar, int Options,

              template <typename, int> class JointCollectionTpl,

              typename ConfigVectorType>

                                const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &list_of_geom_models,

                                const std::vector<JointIndex> &list_of_joints_to_lock,

                                const Eigen::MatrixBase<ConfigVectorType> &reference_configuration)

  {

      typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;

                        reference_configuration, reduced_model,

                        reduced_geom_models);

    }

    {

      using namespace Eigen;

      typedef std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > GeometryModelVector;

              (Model (*)(const Model &, const Model &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>,

              "Append a child model into a parent model, after a specific frame given by its index.\n\n"

              "Parameters:\n"

              "\tmodelA: the parent model\n"

              "\tmodelB: the child model\n"

              "\tframeInModelA:  index of the frame of modelA where to append modelB\n"

              "\taMb: pose of modelB universe joint (index 0) in frameInModelA\n");

              &appendModel_proxy<double,0,JointCollectionDefaultTpl>,

              "Append a child (geometry) model into a parent (geometry) model, after a specific frame given by its index.\n\n"

              "Parameters:\n"

              "\tmodelA: the parent model\n"

              "\tmodelB: the child model\n"

              "\tgeomModelA: the parent geometry model\n"

              "\tgeomModelB: the child geometry model\n"

              "\tframeInModelA:  index of the frame of modelA where to append modelB\n"

              "\taMb: pose of modelB universe joint (index 0) in frameInModelA\n");

              (Model (*)(const Model &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))

              &pinocchio::buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,

                       "list_of_joints_to_lock",

                       "reference_configuration"),

              "Build a reduce model from a given input model and a list of joint to lock.\n\n"

              "Parameters:\n"

              "\tmodel: input kinematic modell to reduce\n"

              "\tlist_of_joints_to_lock: list of joint indexes to lock\n"

              "\treference_configuration: reference configuration to compute the placement of the lock joints\n");

              (bp::tuple (*)(const Model &,

                             const GeometryModel &,

                             const std::vector<JointIndex> &,

                             const Eigen::MatrixBase<VectorXd> &))

              &buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,

                       "geom_model",

                       "list_of_joints_to_lock",

                       "reference_configuration"),

              "Build a reduced model and a reduced geometry model from a given input model,"

              "an input geometry model and a list of joints to lock.\n\n"

              "Parameters:\n"

              "\tmodel: input kinematic model to reduce\n"

              "\tgeom_model: input geometry model to reduce\n"

              "\tlist_of_joints_to_lock: list of joint indexes to lock\n"

              "\treference_configuration: reference configuration to compute the placement of the locked joints\n");

              (bp::tuple(*)(const Model &,

                            const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &,

                            const std::vector<JointIndex> &,

                            const Eigen::MatrixBase<VectorXd> &))

              buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,

                       "reference_configuration"),

              "Build a reduced model and the related reduced geometry models from a given "

              "input model,"

              "a list of input geometry models and a list of joints to lock.\n\n"

              "Parameters:\n"

              "\tmodel: input kinematic model to reduce\n"

              "\tlist_of_geom_models: input geometry models to reduce\n"

              "\tlist_of_joints_to_lock: list of joint indexes to lock\n"

              "\treference_configuration: reference configuration to compute the "

              "placement of the locked joints\n");

  } // namespace python

} // namespace pinocchio