/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @brief wraps Pose3 class to python
 * @author Andrew Melim
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include "gtsam/geometry/Pose3.h"
#include "gtsam/geometry/Pose2.h"
#include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Rot3.h"

using namespace boost::python;
using namespace gtsam;

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transformTo, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transformFrom, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(rotation_overloads, Pose3::rotation, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3)

void exportPose3(){

  // function pointers to desambiguate transformTo() calls
  Point3 (Pose3::*transform_to1)(const Point3&,  OptionalJacobian< 3, 6 >,  OptionalJacobian< 3, 3 > ) const = &Pose3::transformTo;
  Pose3  (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transformTo;
  // function pointers to desambiguate compose() calls
  Pose3 (Pose3::*compose1)(const Pose3 &g) const  = &Pose3::compose;
  Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const  = &Pose3::compose;
  // function pointers to desambiguate between() calls
  Pose3 (Pose3::*between1)(const Pose3 &g) const  = &Pose3::between;
  Pose3 (Pose3::*between2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const  = &Pose3::between;
  // function pointers to desambiguate range() calls
  double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const  = &Pose3::range;
  double (Pose3::*range2)(const Pose3 &,  OptionalJacobian<1,6>, OptionalJacobian<1,6>) const  = &Pose3::range;
  // function pointers to desambiguate bearing() calls
  Unit3 (Pose3::*bearing1)(const Point3 &, OptionalJacobian<2,6>, OptionalJacobian<2,3>) const  = &Pose3::bearing;
  Unit3 (Pose3::*bearing2)(const Pose3 &,  OptionalJacobian<2,6>, OptionalJacobian<2,6>) const  = &Pose3::bearing;

  class_<Pose3>("Pose3")
      .def(init<>())
      .def(init<const Pose3&>())
      .def(init<const Rot3&, const Point3&>())
      .def(init<const Rot3&, const Vector3&>())
      .def(init<const Pose2&>())
      .def(init<const Matrix&>())
      .def("print", &Pose3::print, print_overloads(args("s")))
      .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol")))
      .def("identity", &Pose3::identity)
      .staticmethod("identity")
      .def("matrix", &Pose3::matrix)
      .def("transformFrom", &Pose3::transformFrom,
           transform_from_overloads(args("point", "H1", "H2")))
      .def("transformTo", transform_to1, transform_to_overloads(args("point", "H1", "H2")))
      .def("transformTo", transform_to2)
      .def("x", &Pose3::x)
      .def("y", &Pose3::y)
      .def("z", &Pose3::z)
      .def("rotation", &Pose3::rotation,
           rotation_overloads()[return_value_policy<copy_const_reference>()])
      .def("translation", &Pose3::translation,
           translation_overloads()[return_value_policy<copy_const_reference>()])
      .def(self * self)             // __mult__
      .def(self * other<Point3>())  // __mult__
      .def(self_ns::str(self))      // __str__
      .def(repr(self))              // __repr__
      .def("compose", compose1)
      .def("compose", compose2, compose_overloads())
      .def("between", between1)
      .def("between", between2, between_overloads())
      .def("range", range1, range_overloads())
      .def("range", range2, range_overloads())
      .def("bearing", bearing1, bearing_overloads())
      .def("bearing", bearing2, bearing_overloads());
}