//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2018-2025 INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#pragma once

#include <eigenpy/eigenpy.hpp>
#include <eigenpy/memory.hpp>
#include <eigenpy/copyable.hpp>
#include <boost/python/tuple.hpp>

#include "pinocchio/spatial.hpp"

#include "pinocchio/utils/string.hpp"

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

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    template<typename SE3>
    struct SE3PythonVisitor : public boost::python::def_visitor<SE3PythonVisitor<SE3>>
    {
      typedef typename SE3::Scalar Scalar;
      static constexpr int Options = SE3::Options;
      typedef typename SE3::Matrix3 Matrix3;
      typedef typename SE3::Vector3 Vector3;
      typedef typename SE3::Matrix4 Matrix4;
      typedef typename SE3::Quaternion Quaternion;
      typedef typename SE3::ActionMatrixType ActionMatrixType;

      typedef MotionTpl<Scalar, Options> Motion;
      typedef ForceTpl<Scalar, Options> Force;
      typedef InertiaTpl<Scalar, Options> Inertia;

    public:
      template<class PyClass>
      void visit(PyClass & cl) const
      {
        static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision();

        cl.def(
            bp::init<const Matrix3 &, const Vector3 &>(
              (bp::arg("self"), bp::arg("rotation"), bp::arg("translation")),
              "Initialize from a rotation matrix and a translation vector."))
          .def(
            bp::init<const Quaternion &, const Vector3 &>(
              (bp::arg("self"), bp::arg("quat"), bp::arg("translation")),
              "Initialize from a quaternion and a translation vector."))
          .def(bp::init<int>((bp::arg("self"), bp::arg("int")), "Init to identity."))
          .def(
            bp::init<const Matrix4 &>(
              (bp::arg("self"), bp::arg("array")), "Initialize from an homogeneous matrix."))
          .def(bp::init<const SE3 &>((bp::arg("self"), bp::arg("clone")), "Copy constructor"))

          .add_property(
            "rotation",
            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.")
          .add_property(
            "translation",
            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.")

          .add_property(
            "homogeneous", &SE3::toHomogeneousMatrix,
            "Returns the equivalent homegeneous matrix (acting on SE3).")
          .add_property(
            "action", (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrix,
            "Returns the related action matrix (acting on Motion).")
          .def(
            "toActionMatrix", (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrix,
            bp::arg("self"), "Returns the related action matrix (acting on Motion).")
          .add_property(
            "actionInverse", (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrixInverse,
            "Returns the inverse of the action matrix (acting on Motion).\n"
            "This is equivalent to do m.inverse().action")
          .def(
            "toActionMatrixInverse",
            (ActionMatrixType (SE3::*)() const) & SE3::toActionMatrixInverse, bp::arg("self"),
            "Returns the inverse of the action matrix (acting on Motion).\n"
            "This is equivalent to do m.inverse().toActionMatrix()")
          .add_property(
            "dualAction", (ActionMatrixType (SE3::*)() const) & SE3::toDualActionMatrix,
            "Returns the related dual action matrix (acting on Force).")
          .def(
            "toDualActionMatrix", (ActionMatrixType (SE3::*)() const) & SE3::toDualActionMatrix,
            bp::arg("self"), "Returns the related dual action matrix (acting on Force).")

          .def(
            "setIdentity", &SE3PythonVisitor::setIdentity, bp::arg("self"),
            "Set *this to the identity placement.")
          .def(
            "setRandom", &SE3PythonVisitor::setRandom, bp::arg("self"),
            "Set *this to a random placement.")

          .def("inverse", &SE3::inverse, bp::arg("self"), "Returns the inverse transform")

          .def(
            "act", (Vector3 (SE3::*)(const Vector3 &) const) & SE3::act, bp::args("self", "point"),
            "Returns a point which is the result of the entry point transforms by *this.")
          .def(
            "actInv", (Vector3 (SE3::*)(const Vector3 &) const) & SE3::actInv,
            bp::args("self", "point"),
            "Returns a point which is the result of the entry point by the inverse of *this.")

          .def(
            "act", (SE3 (SE3::*)(const SE3 & other) const) & SE3::act, bp::args("self", "M"),
            "Returns the result of *this * M.")
          .def(
            "actInv", (SE3 (SE3::*)(const SE3 & other) const) & SE3::actInv, bp::args("self", "M"),
            "Returns the result of the inverse of *this times M.")

          .def(
            "act", (Motion (SE3::*)(const Motion &) const) & SE3::act, bp::args("self", "motion"),
            "Returns the result action of *this onto a Motion.")
          .def(
            "actInv", (Motion (SE3::*)(const Motion &) const) & SE3::actInv,
            bp::args("self", "motion"), "Returns the result of the inverse of *this onto a Motion.")

          .def(
            "act", (Force (SE3::*)(const Force &) const) & SE3::act, bp::args("self", "force"),
            "Returns the result of *this onto a Force.")
          .def(
            "actInv", (Force (SE3::*)(const Force &) const) & SE3::actInv,
            bp::args("self", "force"),
            "Returns the result of the inverse of *this onto an Inertia.")

          .def(
            "act", (Inertia (SE3::*)(const Inertia &) const) & SE3::act,
            bp::args("self", "inertia"), "Returns the result of *this onto a Force.")
          .def(
            "actInv", (Inertia (SE3::*)(const Inertia &) const) & SE3::actInv,
            bp::args("self", "inertia"),
            "Returns the result of the inverse of *this onto an Inertia.")

#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
          .def(
            "isApprox", &SE3::isApprox,
            (bp::arg("self"), bp::arg("other"), bp::arg("prec") = dummy_precision),
            "Returns true if *this is approximately equal to other, within the precision given by "
            "prec.")

          .def(
            "isIdentity", &SE3::isIdentity, (bp::arg("self"), bp::arg("prec") = dummy_precision),
            "Returns true if *this is approximately equal to the identity placement, within the "
            "precision given by prec.")
#endif

          .def("__invert__", &SE3::inverse, "Returns the inverse of *this.")
          .def(bp::self * bp::self)
          .def("__mul__", &__mul__<Motion>)
          .def("__mul__", &__mul__<Force>)
          .def("__mul__", &__mul__<Inertia>)
          .def("__mul__", &__mul__<Vector3>)
          .add_property("np", &SE3::toHomogeneousMatrix)

#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
          .def(bp::self == bp::self)
          .def(bp::self != bp::self)
#endif

          .def("Identity", &SE3::Identity, "Returns the identity transformation.")
          .staticmethod("Identity")
          .def("Random", &SE3::Random, "Returns a random transformation.")
          .staticmethod("Random")
          .def(
            "Interpolate", &SE3::template Interpolate<Scalar>, 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")
          .staticmethod("Interpolate")

          .def("__array__", &SE3::toHomogeneousMatrix)
          .def(
            "__array__", &__array__,
            (bp::arg("self"), bp::arg("dtype") = bp::object(), bp::arg("copy") = bp::object()))

#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
          .def_pickle(Pickle())
#endif
          ;
      }

      static std::string scopeName()
      {
        static std::string scope_name;
        return scope_name;
      }

      static void expose()
      {
        typedef ::boost::python::detail::not_specified HolderType;
        bp::class_<SE3, HolderType>(
          "SE3", "SE3 transformation defined by a 3d vector and a rotation matrix.",
          bp::init<>(bp::arg("self"), "Default constructor."))
          .def(SE3PythonVisitor<SE3>())
          .def(CastVisitor<SE3>())
          .def(ExposeConstructorByCastVisitor<SE3, ::pinocchio::SE3>())
          .def(::eigenpy::CopyableVisitor<SE3>())
          .def(bp::self_ns::str(bp::self_ns::self))
          .def("__repr__", &repr);
      }

    private:
      static Matrix4 __array__(const SE3 & self, bp::object, bp::object)
      {
        return self.toHomogeneousMatrix();
      }

      struct Pickle : bp::pickle_suite
      {
        static boost::python::tuple getinitargs(const SE3 & M)
        {
          return bp::make_tuple((Matrix3)M.rotation(), (Vector3)M.translation());
        }

        static bool getstate_manages_dict()
        {
          return true;
        }
      };

      static void setIdentity(SE3 & self)
      {
        self.setIdentity();
      }
      static void setRandom(SE3 & self)
      {
        self.setRandom();
      }

      template<typename Spatial>
      static Spatial __mul__(const SE3 & self, const Spatial & other)
      {
        return self.act(other);
      }

      static std::string repr(const SE3 & self)
      {
        //        bp::object
        //        py_rotation(bp::handle<>(eigenpy::EigenToPy<Matrix3,Scalar>::convert(self.rotation())));
        //        std::string rotation_repr =
        //        bp::extract<std::string>(py_rotation.attr("__repr__")());
        //
        //        bp::object
        //        py_translation(bp::handle<>(eigenpy::EigenToPy<Vector3,Scalar>::convert(self.translation())));
        //        std::string translation_repr =
        //        bp::extract<std::string>(py_translation.attr("__repr__")());

        bp::object py_homogeneous(
          bp::handle<>(eigenpy::EigenToPy<Matrix4, Scalar>::convert(self.toHomogeneousMatrix())));
        std::string homegeneous_repr = bp::extract<std::string>(py_homogeneous.attr("__repr__")());
        replace(homegeneous_repr, "\n", "");
        replace(homegeneous_repr, "       ", "");

        std::stringstream ss_repr;
        ss_repr << "SE3(";
        ss_repr << homegeneous_repr;
        ss_repr << ")";

        return ss_repr.str();
      }
    };

  } // namespace python
} // namespace pinocchio
