/**
 *@file  Pose3.h
 *@brief 3D Pose
 */

// \callgraph

#pragma once

#include "Point3.h"
#include "Rot3.h"

namespace gtsam {

	/** A 3D pose (R,t) : (Rot3,Point3) */
	class Pose3 {
	private:
		Rot3 R_;
		Point3 t_;

	public:
		Pose3() {
		} // default is origin
		Pose3(const Pose3& pose) :
			R_(pose.R_), t_(pose.t_) {
		}
		Pose3(const Rot3& R, const Point3& t) :
			R_(R), t_(t) {
		}

		/** constructor from 4*4 matrix */
		Pose3(const Matrix &T) :
			R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
					T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
		}

		/** constructor from 12D vector */
		Pose3(const Vector &V) :
			R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), t_(V(9), V(10),
					V(11)) {
		}

		const Rot3& rotation() const {
			return R_;
		}

		const Point3& translation() const {
			return t_;
		}

		/** return DOF, dimensionality of tangent space = 6 */
		size_t dim() const {
			return 6;
		}

		/** Given 6-dim tangent vector, create new pose */
		Pose3 exmap(const Vector& d) const;

		/** inverse transformation */
		Pose3 inverse() const;

		/** composition */
		inline Pose3 operator*(const Pose3& B) const {
			return Pose3(matrix() * B.matrix());
		}

		/** return 12D vectorized form (column-wise) */
		Vector vector() const {
			Vector r = R_.vector(), t = t_.vector();
			return concatVectors(2, &r, &t);
		}

		/** convert to 4*4 matrix */
		Matrix matrix() const {
			const double row4[] = { 0, 0, 0, 1 };
			Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4);
			return stack(2, &A34, &A14);
		}

		/** print with optional string */
		void print(const std::string& s = "") const {
			R_.print(s + ".R");
			t_.print(s + ".t");
		}

		/** transforms */
		Pose3 transformPose_to(const Pose3& transform) const;

		/** assert equality up to a tolerance */
		bool equals(const Pose3& pose, double tol = 1e-9) const;

		/** friends */
		friend Point3 transform_from(const Pose3& pose, const Point3& p);
		friend Point3 transform_to(const Pose3& pose, const Point3& p);
		friend Pose3 composeTransform(const Pose3& current, const Pose3& target);

	private:
		/** Serialization function */
		friend class boost::serialization::access;
		template<class Archive>
		void serialize(Archive & ar, const unsigned int version) {
			ar & BOOST_SERIALIZATION_NVP(R_);
			ar & BOOST_SERIALIZATION_NVP(t_);
		}
	}; // Pose3 class

	/** finds a transform from the current frame to the target frame given two coordinates of the same point */
	Pose3 composeTransform(const Pose3& current, const Pose3& target);

	/** receives the point in Pose coordinates and transforms it to world coordinates */
	Point3 transform_from(const Pose3& pose, const Point3& p);
	Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
	Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !

	/** receives the point in world coordinates and transforms it to Pose coordinates */
	Point3 transform_to(const Pose3& pose, const Point3& p);
	Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
	Matrix Dtransform_to2(const Pose3& pose); // does not depend on p !

	/** direct measurement of a pose */
	Vector hPose(const Vector& x);

	/**
	 * derivative of direct measurement
	 * 12*6, entry i,j is how measurement error will change
	 */
	Matrix DhPose(const Vector& x);

	/** assert equality up to a tolerance */
	bool assert_equal(const Pose3& A, const Pose3& B, double tol = 1e-9);

} // namespace gtsam