#pragma once

#include "../../../include/dv-processing/kinematics/transformation.hpp"

#include <Eigen/Geometry>
#include <pybind11/pybind11.h>

namespace py = pybind11;

/**
 * @brief Bind kinematics::Transformationf class to python
 * @param m pybind The module to bind to
 */
void bind_transformation(py::module &m) {
	py::class_<dv::kinematics::Transformationf>(
		m, "Transformationf", "Basic transformation wrapper containing homogenous 3D transformation and timestamp.")
		.def(py::init<>(), "Construct an identity transformation from with timestamp.")
		.def(py::init<int64_t, const Eigen::Matrix4f &>(), py::arg("timestamp"), py::arg("transformation"),
			"Construct the transformation from a timestamp and a homogenous 4x4 transformation matrix.")
		.def(py::init([](int64_t timestamp, const Eigen::Vector3f &translation, const dv::Quaternion &rotation) {
			return dv::kinematics::Transformationf(
				timestamp, translation, Eigen::Quaternionf(rotation.w(), rotation.x(), rotation.y(), rotation.z()));
		}),
			py::arg("timestamp"), py::arg("translation"), py::arg("rotation"),
			"Construct the transformation from timestamp, 3D translation vector and quaternion describing the "
			"rotation.")
		.def(py::init<int64_t, const Eigen::Vector3f &, const Eigen::Matrix3f &>(), py::arg("timestamp"),
			py::arg("translation"), py::arg("rotation"),
			"Construct the transformation from timestamp, 3D translation vector and a rotation matrix describing the "
			"rotation.")
		.def("getTimestamp", &dv::kinematics::Transformationf::getTimestamp,
			"Unix timestamp of the transformation in microseconds.")
		.def("getTransform", &dv::kinematics::Transformationf::getTransform, "Get the transformation matrix.")
		.def("getRotationMatrix", &dv::kinematics::Transformationf::getRotationMatrix,
			"Retrieve a copy of 3x3 rotation matrix.")
		.def(
			"getQuaternion",
			[](const dv::kinematics::Transformationf &self) {
				const auto quat = self.getQuaternion();
				return dv::Quaternion(quat.w(), quat.x(), quat.y(), quat.z());
			},
			"Retrieve rotation expressed as a quaternion.")
		.def("getTranslation", &dv::kinematics::Transformationf::getTranslation<Eigen::Vector3f>,
			"Retrieve translation as 3D vector.")
		.def("transformPoint", &dv::kinematics::Transformationf::transformPoint<Eigen::Vector3f, Eigen::Vector3f>,
			py::arg("point"), "Transform a point using this transformation.")
		.def("rotatePoint", &dv::kinematics::Transformationf::rotatePoint<Eigen::Vector3f, Eigen::Vector3f>,
			py::arg("point"), "Apply rotation only transformation on the given point.")
		.def("inverse", &dv::kinematics::Transformationf::inverse,
			"Calculate the inverse homogenous transformation of this transform.")
		.def("delta", &dv::kinematics::Transformationf::delta, py::arg("target"),
			"Get a transformation from this transformation into the given other transformation.");
}
