//

// Copyright (c) 2015-2023 CNRS INRIA

// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.

//

#ifndef __pinocchio_python_spatial_inertia_hpp__

#define __pinocchio_python_spatial_inertia_hpp__

#include <eigenpy/exception.hpp>

#include <eigenpy/eigenpy.hpp>

#include <eigenpy/memory.hpp>

#include <boost/python/tuple.hpp>

#include "pinocchio/spatial/inertia.hpp"

#include "pinocchio/bindings/python/fwd.hpp"

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

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

#if EIGENPY_VERSION_AT_MOST(2,8,1)

EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Inertia)

#endif

namespace pinocchio

{

  namespace python

  {

    namespace bp = boost::python;

    template<typename T> struct call;

    template<typename Scalar, int Options>

    struct call< InertiaTpl<Scalar,Options> >

    {

      typedef InertiaTpl<Scalar,Options> Inertia;

                           const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())

      {

      }

                         const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())

      {

      }

    };

    template<typename Inertia>

    struct InertiaPythonVisitor

      : public boost::python::def_visitor< InertiaPythonVisitor<Inertia> >

    {

      typedef typename Inertia::Scalar Scalar;

      typedef typename Inertia::Vector3 Vector3;

      typedef typename Inertia::Matrix3 Matrix3;

      typedef typename Inertia::Vector6 Vector6;

      typedef typename Inertia::Matrix6 Matrix6;

    public:

      template<class PyClass>

      {

        .def("__init__",

             bp::make_constructor(&InertiaPythonVisitor::makeFromMCI,

                                  bp::default_call_policies(),

                                  bp::args("mass","lever","inertia")),

             "Initialize from mass, lever and 3d inertia.")

                      &InertiaPythonVisitor::getMass,

                      &InertiaPythonVisitor::setMass,

                      "Mass of the Spatial Inertia.")

                      bp::make_function((typename Inertia::Vector3 & (Inertia::*)())&Inertia::lever,

                                        bp::return_internal_reference<>()),

                      &InertiaPythonVisitor::setLever,

                      "Center of mass location of the Spatial Inertia. It corresponds to the location of the center of mass regarding to the frame where the Spatial Inertia is expressed.")

                      &InertiaPythonVisitor::getInertia,

                      &InertiaPythonVisitor::setInertia,

                      "Rotational part of the Spatial Inertia, i.e. a symmetric matrix representing the rotational inertia around the center of mass.")

             bp::args("self","M"),"Returns the result of the action of M on *this.")

             bp::args("self","M"),"Returns the result of the action of the inverse of M on *this.")

             "Set *this to be the Identity inertia.")

             "Set all the components of *this to zero.")

             "Set all the components of *this to random values.")

             bp::args("self","v"),

             "Returns the result of v x* I, a 6x6 matrix.")

             bp::args("self","v"),

             "Returns the result of I vx, a 6x6 matrix.")

             bp::args("self","v"),

             "Returns the time derivative of the inertia.")

             call<Inertia>::isApprox,

             isApproxInertia_overload(bp::args("self","other","prec"),

                                      "Returns true if *this is approximately equal to other, within the precision given by prec."))

             call<Inertia>::isZero,

             isZero_overload(bp::args("self","prec"),

                             "Returns true if *this is approximately equal to the zero Inertia, within the precision given by prec."))

             "Returns the representation of the matrix as a vector of dynamic parameters."

              "\nThe parameters are given as v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "

              "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter"

        )

              bp::args("dynamic_parameters"),

              "Builds and inertia matrix from a vector of dynamic parameters."

              "\nThe parameters are given as dynamic_parameters = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "

              "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter."

        )

             bp::args("mass","radius"),

             "Returns the Inertia of a sphere defined by a given mass and radius.")

             bp::args("mass","length_x","length_y","length_z"),

             "Returns the Inertia of an ellipsoid shape defined by a mass and given dimensions the semi-axis of values length_{x,y,z}.")

             bp::args("mass","radius","length"),

             "Returns the Inertia of a cylinder defined by its mass, radius and length along the Z axis.")

             bp::args("mass","length_x","length_y","length_z"),

             "Returns the Inertia of a box shape with a mass and of dimension the semi axis of length_{x,y,z}.")

        ;

//      static void setInertia(Inertia & self, const Vector6 & minimal_inertia) { self.inertia().data() = minimal_inertia; }

      {

      }

      {

      }

                                  const Vector3 & lever,

                                  const Matrix3 & inertia)

      {

           }

      {

#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6 && EIGENPY_VERSION_AT_LEAST(2,9,0)

    typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(Inertia) HolderType;

#else

    typedef ::boost::python::detail::not_specified HolderType;

#endif

                            "This class represenses a sparse version of a Spatial Inertia and its is defined by its mass, its center of mass location and the rotational inertia expressed around this center of mass.\n\n"

                            "Supported operations ...",

                            bp::init<>(bp::arg("self"),"Default constructor."))

        .def(InertiaPythonVisitor<Inertia>())

        ;

    private:

      struct Pickle : bp::pickle_suite

      {

        static

        boost::python::tuple

      };

    }; // struct InertiaPythonVisitor

  } // namespace python

} // namespace pinocchio

#endif // ifndef __pinocchio_python_spatial_inertia_hpp__