/**
 *@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