//

// Copyright (c) 2015-2023 CNRS INRIA

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

//

#ifndef __pinocchio_python_spatial_se3_hpp__

#define __pinocchio_python_spatial_se3_hpp__

#include <eigenpy/eigenpy.hpp>

#include <eigenpy/memory.hpp>

#include <boost/python/tuple.hpp>

#include "pinocchio/spatial/se3.hpp"

#include "pinocchio/spatial/motion.hpp"

#include "pinocchio/spatial/force.hpp"

#include "pinocchio/spatial/inertia.hpp"

#include "pinocchio/spatial/explog.hpp"

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

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

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

EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::SE3)

namespace pinocchio

{

  namespace python

  {

    namespace bp = boost::python;

    template<typename T> struct call;

    template<typename Scalar, int Options>

    struct call< SE3Tpl<Scalar,Options> >

    {

      typedef SE3Tpl<Scalar,Options> SE3;

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

      {

      }

    };

    template<typename SE3>

    struct SE3PythonVisitor

    : public boost::python::def_visitor< SE3PythonVisitor<SE3> >

    {

      typedef typename SE3::Scalar Scalar;

      typedef typename SE3::Matrix3 Matrix3;

      typedef typename SE3::Vector3 Vector3;

      typedef typename SE3::Matrix4 Matrix4;

      typedef typename SE3::Quaternion Quaternion;

    public:

      template<class PyClass>

      {

        .def(bp::init<Matrix3,Vector3>

             (bp::args("self","rotation","translation"),

              "Initialize from a rotation matrix and a translation vector."))

             (bp::args("self","quat","translation"),

              "Initialize from a quaternion and a translation vector."))

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

              "Initialize from an homogeneous matrix."))

                      bp::make_function((typename SE3::AngularRef (SE3::*)()) &SE3::rotation,bp::return_internal_reference<>()),

                      (void (SE3::*)(const Matrix3 &)) &SE3::rotation,

                      "The rotation part of the transformation.")

                      bp::make_function((typename SE3::LinearRef (SE3::*)()) &SE3::translation,bp::return_internal_reference<>()),

                      (void (SE3::*)(const Vector3 &)) &SE3::translation,

                      "The translation part of the transformation.")

                      "Returns the equivalent homegeneous matrix (acting on SE3).")

                      "Returns the related action matrix (acting on Motion).")

             "Returns the related action matrix (acting on Motion).")

                      "Returns the inverse of the action matrix (acting on Motion).\n"

                      "This is equivalent to do m.inverse().action")

             "Returns the inverse of the action matrix (acting on Motion).\n"

             "This is equivalent to do m.inverse().toActionMatrix()")

                      "Returns the related dual action matrix (acting on Force).")

             "Returns the related dual action matrix (acting on Force).")

             "Set *this to the identity placement.")

             "Set *this to a random placement.")

             "Returns the inverse transform")

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

             "Returns a point which is the result of the entry point transforms by *this.")

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

             "Returns a point which is the result of the entry point by the inverse of *this.")

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

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

             bp::args("self","motion"), "Returns the result action of *this onto a Motion.")

             bp::args("self","motion"), "Returns the result of the inverse of *this onto a Motion.")

             bp::args("self","force"), "Returns the result of *this onto a Force.")

             bp::args("self","force"), "Returns the result of the inverse of *this onto an Inertia.")

             bp::args("self","inertia"), "Returns the result of *this onto a Force.")

             bp::args("self","inertia"), "Returns the result of the inverse of *this onto an Inertia.")

             call<SE3>::isApprox,

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

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

             &SE3::isIdentity,

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

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

             bp::args("A","B","alpha"),

             "Linear interpolation on the SE3 manifold.\n\n"

             "This method computes the linear interpolation between A and B, such that the result C = A + (B-A)*t if it would be applied on classic Euclidian space.\n"

             "This operation is very similar to the SLERP operation on Rotations.\n"

             "Parameters:\n"

             "\tA: Initial transformation\n"

             "\tB: Target transformation\n"

             "\talpha: Interpolation factor")

        ;

      {

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

    typedef PINOCCHIO_SHARED_PTR_HOLDER_TYPE(SE3) HolderType;

#else

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

#endif

                        "SE3 transformation defined by a 3d vector and a rotation matrix.",

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

        .def(SE3PythonVisitor<SE3>())

        ;

    private:

      struct Pickle : bp::pickle_suite

      {

        static

        boost::python::tuple

      };

      template<typename Spatial>

    };

  } // namespace python

} // namespace pinocchio

#endif // ifndef __pinocchio_python_spatial_se3_hpp__