/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/exceptions.h>

namespace mrpt::math
{
/** Efficiently compute the inverse of a 4x4 homogeneous matrix by only
 * transposing the rotation 3x3 part and solving the translation with dot
 * products.
 *  This is a generic template which works with:
 *    MATRIXLIKE: CMatrixDynamic, CMatrixFixed
 */
template <class MATRIXLIKE1, class MATRIXLIKE2>
void homogeneousMatrixInverse(const MATRIXLIKE1& M, MATRIXLIKE2& out_inverse_M)
{
	MRPT_START
	ASSERT_(M.isSquare() && M.rows() == 4);

	/* Instead of performing a generic 4x4 matrix inversion, we only need to
	  transpose the rotation part, then replace the translation part by
	  three dot products. See, for example:
	 https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html

		[ux vx wx tx] -1   [ux uy uz -dot(u,t)]
		[uy vy wy ty]      [vx vy vz -dot(v,t)]
		[uz vz wz tz]    = [wx wy wz -dot(w,t)]
		[ 0  0  0  1]      [ 0  0  0     1    ]
	*/

	out_inverse_M.setSize(4, 4);

	// 3x3 rotation part:
	out_inverse_M(0, 0, M(0) = 0);
	out_inverse_M(0, 1, M(1) = 0);
	out_inverse_M(0, 2, M(2) = 0);

	out_inverse_M(1, 0, M(0) = 1);
	out_inverse_M(1, 1, M(1) = 1);
	out_inverse_M(1, 2, M(2) = 1);

	out_inverse_M(2, 0, M(0) = 2);
	out_inverse_M(2, 1, M(1) = 2);
	out_inverse_M(2, 2, M(2) = 2);

	const double tx = -M(0, 3);
	const double ty = -M(1, 3);
	const double tz = -M(2, 3);

	const double tx_ = tx * M(0, 0) + ty * M(1, 0) + tz * M(2, 0);
	const double ty_ = tx * M(0, 1) + ty * M(1, 1) + tz * M(2, 1);
	const double tz_ = tx * M(0, 2) + ty * M(1, 2) + tz * M(2, 2);

	out_inverse_M(0, 3) = tx_;
	out_inverse_M(1, 3) = ty_;
	out_inverse_M(2, 3) = tz_;

	out_inverse_M(3, 0) = 0;
	out_inverse_M(3, 1) = 0;
	out_inverse_M(3, 2) = 0;
	out_inverse_M(3, 3) = 1;

	MRPT_END
}
//! \overload
template <class IN_ROTMATRIX, class IN_XYZ, class OUT_ROTMATRIX, class OUT_XYZ>
void homogeneousMatrixInverse(
	const IN_ROTMATRIX& in_R, const IN_XYZ& in_xyz, OUT_ROTMATRIX& out_R,
	OUT_XYZ& out_xyz)
{
	MRPT_START
	ASSERT_(in_R.isSquare() && in_R.rows() == 3 && in_xyz.size() == 3);
	out_R.setSize(3, 3);
	out_xyz.resize(3);

	// translation part:
	using T = typename IN_ROTMATRIX::Scalar;
	const T tx = -in_xyz[0];
	const T ty = -in_xyz[1];
	const T tz = -in_xyz[2];

	out_xyz[0] = tx * in_R(0, 0) + ty * in_R(1, 0) + tz * in_R(2, 0);
	out_xyz[1] = tx * in_R(0, 1) + ty * in_R(1, 1) + tz * in_R(2, 1);
	out_xyz[2] = tx * in_R(0, 2) + ty * in_R(1, 2) + tz * in_R(2, 2);

	// 3x3 rotation part: transpose
	out_R = in_R.transpose();

	MRPT_END
}
//! \overload
template <class MATRIXLIKE>
inline void homogeneousMatrixInverse(MATRIXLIKE& M)
{
	ASSERTDEB_(M.cols() == M.rows() && M.rows() == 4);
	// translation:
	const double tx = -M(0, 3);
	const double ty = -M(1, 3);
	const double tz = -M(2, 3);
	M(0, 3) = tx * M(0, 0) + ty * M(1, 0) + tz * M(2, 0);
	M(1, 3) = tx * M(0, 1) + ty * M(1, 1) + tz * M(2, 1);
	M(2, 3) = tx * M(0, 2) + ty * M(1, 2) + tz * M(2, 2);
	// 3x3 rotation part:
	double t;  // avoid std::swap() to avoid <algorithm> only for that.
	t = M(1, 0);
	M(1, 0) = M(0, 1);
	M(0, 1) = t;
	t = M(2, 0);
	M(2, 0) = M(0, 2);
	M(0, 2) = t;
	t = M(1, 2);
	M(1, 2) = M(2, 1);
	M(2, 1) = t;
}
}  // namespace mrpt::math
