// rotmatrix.cpp (RotMatrix<> implementation)
//
//  The WorldForge Project
//  Copyright (C) 2001  The WorldForge Project
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//  For information about WorldForge and its authors, please contact
//  the Worldforge Web Site at http://www.worldforge.org.

// Author: Ron Steinke
// Created: 2001-12-7

#include "rotmatrix_funcs.h"
#include "quaternion.h"

#include <limits>

namespace WFMath {

static CoordType _MatrixDeterminantImpl(int size, CoordType* m);

template<>
RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
										   const bool not_flip) {
	CoordType xx, yy, zz, xy, xz, yz;
	const Vector<3>& vref = q.vector();

	xx = vref[0] * vref[0];
	xy = vref[0] * vref[1];
	xz = vref[0] * vref[2];
	yy = vref[1] * vref[1];
	yz = vref[1] * vref[2];
	zz = vref[2] * vref[2];

	Vector<3> wvec = vref * q.scalar();

	m_elem[0][0] = 1 - 2 * (yy + zz);
	m_elem[1][1] = 1 - 2 * (xx + zz);
	m_elem[2][2] = 1 - 2 * (xx + yy);

	m_elem[0][1] = 2 * (xy - wvec[2]);
	m_elem[0][2] = 2 * (xz + wvec[1]);

	m_elem[1][0] = 2 * (xy + wvec[2]);
	m_elem[1][2] = 2 * (yz - wvec[0]);

	m_elem[2][0] = 2 * (xz - wvec[1]);
	m_elem[2][1] = 2 * (yz + wvec[0]);

	m_flip = !not_flip;
	m_age = q.age();
	if (!not_flip)
		*this = Prod(*this, RotMatrix<3>().mirror(0));

	m_valid = true;
	return *this;
}

template<>
RotMatrix<3>& RotMatrix<3>::rotate(const Quaternion& q) {
	Vector<3> vec;
	vec.setValid();
	m_valid = m_valid && q.isValid();
	m_age += q.age();

	// rotate both sides by q

	for (auto& vec_num: m_elem) {
		for (int elem_num = 0; elem_num < 3; ++elem_num)
			vec[elem_num] = vec_num[elem_num];
		vec.rotate(q);
		for (int elem_num = 0; elem_num < 3; ++elem_num)
			vec_num[elem_num] = vec[elem_num];
	}

	checkNormalization();

	return *this;
}

template<>
RotMatrix<3>& RotMatrix<3>::rotation(const Vector<3>& axis,
									 CoordType theta) {
	CoordType max = 0;
	int main_comp = -1;

	for (int i = 0; i < 3; ++i) {
		CoordType val = std::fabs(axis[i]);
		if (val > max) {
			max = val;
			main_comp = i;
		}
	}

	assert("Must pass a nonzero length vector as axis to avoid this" && main_comp != -1);

	Vector<3> tmp, v1, v2;

	int new_comp = main_comp ? main_comp - 1 : 2; // Not parallel to axis
	for (int i = 0; i < 3; ++i)
		tmp[i] = ((i == new_comp) ? 1.f : 0.f);

	v1 = Cross(axis, tmp); // 3D specific part
	v2 = Cross(axis, v1);

	return rotation(v1, v2, theta);
}

template<>
RotMatrix<3>& RotMatrix<3>::rotation(const Vector<3>& axis) {
	CoordType max = 0;
	int main_comp = -1;
	CoordType angle = axis.mag();

	if (angle == 0) {
		return identity();
	}

	for (int i = 0; i < 3; ++i) {
		CoordType val = std::fabs(axis[i]);
		if (val > max) {
			max = val;
			main_comp = i;
		}
	}

	assert("Can't happen with nonzero angle" && main_comp != -1);

	Vector<3> tmp, v1, v2;

	int new_comp = main_comp ? main_comp - 1 : 2; // Not parallel to axis
	for (int i = 0; i < 3; ++i)
		tmp[i] = ((i == new_comp) ? 1.f : 0.f);

	v1 = Cross(axis, tmp); // 3D specific part
	v2 = Cross(axis, v1);

	return rotation(v1, v2, angle);
}

bool _MatrixSetValsImpl(const int size, CoordType* vals, bool& flip,
						CoordType* buf1, CoordType* buf2, CoordType precision) {
	precision = std::fabs(precision);

	if (precision >= .9) // Can get an infinite loop for precision == 1
		return false;

	// Check that vals form an orthogonal matrix, also increase their
	// precision to WFMATH_EPSILON

	while (true) {
		CoordType try_prec = 0;

		for (int i = 0; i < size; ++i) {
			for (int j = 0; j < size; ++j) {
				CoordType ans = 0;
				for (int k = 0; k < size; ++k) {
					ans += vals[i * size + k] * vals[j * size + k];
				}

				if (i == j) // Subtract identity matrix
					--ans;
				ans = std::fabs(ans);
				if (ans >= try_prec)
					try_prec = ans;
			}
		}

		if (try_prec > precision)
			return false;

		if (try_prec <= numeric_constants<CoordType>::epsilon())
			break;

		// Precision needs improvement, use linear approximation scheme.

		// This scheme takes the original matrix (call it A)
		// and subtracts another matrix (delta), where
		//
		// delta = (A - (A^T)^-1) / 2
		//
		// This is correct, up to errors of order delta^2,
		// if you assume you can choose a delta such that
		// A^T * delta is symmetric (delta is underdetermined
		// by the linear approximation scheme)

		// This procedure will not increase the precision of
		// the parameters which determine the matrix (i.e. the
		// Euler angles), but it will increase the precision
		// to which the matrix satisfies the condition
		// A^T * A == (identity matrix).

		// The symmetry condition on A^T * delta represents an
		// arbitrary condition which will determine the higher
		// precision components of the Euler angles. This
		// makes the problem of increasing the precision of
		// the matrix well determined, and solvable by this
		// perturbative method.

		for (int i = 0; i < size; ++i) {
			for (int j = 0; j < size; ++j) {
				buf1[i * size + j] = vals[j * size + i];
				buf2[i * size + j] = ((i == j) ? 1.f : 0.f);
			}
		}

		bool ans = _MatrixInverseImpl(size, buf1, buf2);

		if (!ans) { // Degenerate matrix, something badly wrong
			return false;
		}

		for (int i = 0; i < size; ++i) {
			for (int j = 0; j < size; ++j) {
				CoordType& elem = vals[i * size + j];
				elem += buf2[i * size + j];
				elem /= 2;
			}
		}

		// The above scheme should approx. square the precision.
		// That is, try_prec -> try_prec * try_prec * (fudge factor)
		// in the next iteration.
	}

	// The determinant is either 1 or -1, depending on the parity.
	// Use that to calculate flip.

	for (int i = 0; i < size; ++i)
		for (int j = 0; j < size; ++j)
			buf1[i * size + j] = vals[i * size + j];

	flip = _MatrixDeterminantImpl(size, buf1) < 0;

	return true;
}

static CoordType _MatrixDeterminantImpl(const int size, CoordType* m) {
	// First, construct an upper triangular matrix with the
	// same determinant as the original matrix. Then just
	// multiply the diagonal terms to get the determinant.

	for (int i = 0; i < size - 1; ++i) {
		CoordType minval = 0;
		for (int j = 0; j < size; ++j)
			minval += m[j * size + i] * m[j * size + i];
		minval /= std::numeric_limits<CoordType>::max();
		if (minval < std::numeric_limits<CoordType>::min())
			minval = std::numeric_limits<CoordType>::min();
		if (m[i * size + i] * m[i * size + i] < minval) { // Find a row with nonzero element
			int j;
			for (j = i + 1; j < size; ++j)
				if (m[j * size + i] * m[j * size + i] >= minval)
					break;
			if (j == size) // No nonzero element found, degenerate matrix, det == 0
				return 0;
			m[i * size + i] = m[j * size + i];
			for (int k = i + 1; k < size; ++k) // For k < i, m[j*size+k] == 0
				m[i * size + k] += m[j * size + k];
		}
		for (int j = i + 1; j < size; ++j) {
			CoordType factor = m[j * size + i] / m[i * size + i];
			// We know factor isn't bigger than about sqrt(WFMATH_MAX), due to
			// the comparison with minval done above.
			m[j * size + i] = 0;
			if (factor != 0) {
				for (int k = i + 1; k < size; ++k) // For k < i, m[k*size+j] == 0
					m[j * size + k] -= m[i * size + k] * factor;
			}
		}
	}

	CoordType out = 1;

	for (int i = 0; i < size; ++i)
		out *= m[i * size + i];

	return out;
}

bool _MatrixInverseImpl(const int size, CoordType* in, CoordType* out) {
	// Invert using row operations. First, make m upper triangular,
	// with 1's on the diagonal

	for (int i = 0; i < size; ++i) {

		// Make sure in[i*size+i] is nonzero
		CoordType minval = 0;
		for (int j = 0; j < size; ++j)
			minval += in[j * size + i] * in[j * size + i];
		minval /= std::numeric_limits<CoordType>::max();
		if (minval < std::numeric_limits<CoordType>::min())
			minval = std::numeric_limits<CoordType>::min();
		if (in[i * size + i] * in[i * size + i] < minval) { // Find a nonzero element
			int j;
			for (j = i + 1; j < size; ++j)
				if (in[j * size + i] * in[j * size + i] >= minval)
					break;
			if (j == size) // degenerate matrix
				return false;
			for (int k = 0; k < size; ++k) {
				out[i * size + k] += out[j * size + k];
				in[i * size + k] += in[j * size + k];
			}
		}
		// We now know in[i*size+i] / in[j*size+i] >= sqrt(WFMATH_MIN) for any j

		// Normalize the row, so in[i*size+i] == 1
		CoordType tmp = in[i * size + i];
		in[i * size + i] = 1;
		for (int j = 0; j < size; ++j) {
			out[i * size + j] /= tmp;
			if (j > i) // in[i*size+j] == 0 for j < i
				in[i * size + j] /= tmp;
		}

		// Do row subtraction to make in[j*size+i] zero for j > i
		for (int j = i + 1; j < size; ++j) {
			CoordType tmp2 = in[j * size + i];
			in[j * size + i] = 0;
			if (tmp2 != 0) {
				for (int k = 0; k < size; ++k) {
					out[j * size + k] -= out[i * size + k] * tmp2;
					in[j * size + k] -= in[i * size + k] * tmp2;
				}
			}
		}
	}

	// Now perform row operations on "out" which would make "m"
	// into the identity matrix

	for (int i = size - 1; i >= 1; --i) {
		for (int j = i - 1; j >= 0; --j) {
			CoordType tmp = in[j * size + i];
			if (tmp != 0)
				for (int k = 0; k < size; ++k)
					out[j * size + k] -= out[i * size + k] * tmp;
			// Don't bother modifying in[j*size+k], we never use it again.
		}
	}

	return true;
}

template<>
RotMatrix<3>::RotMatrix(const Quaternion& q,
						const bool not_flip) : m_flip(false), m_valid(false),
											   m_age(0) {
	fromQuaternion(q, not_flip);
}

template
class RotMatrix<2>;

template
class RotMatrix<3>;

static_assert(std::is_standard_layout<RotMatrix<2>>::value, "RotMatrix should be standard layout.");
static_assert(std::is_trivially_copyable<RotMatrix<2>>::value, "RotMatrix should be trivially copyable.");

static_assert(std::is_standard_layout<RotMatrix<3>>::value, "RotMatrix should be standard layout.");
static_assert(std::is_trivially_copyable<RotMatrix<3>>::value, "RotMatrix should be trivially copyable.");

template RotMatrix<2> operator* <2>(RotMatrix<2> const&, RotMatrix<2> const&);

template RotMatrix<3> operator* <3>(RotMatrix<3> const&, RotMatrix<3> const&);

template RotMatrix<2> ProdInv<2>(RotMatrix<2> const&, RotMatrix<2> const&);

template RotMatrix<3> ProdInv<3>(RotMatrix<3> const&, RotMatrix<3> const&);

template Vector<2> operator* <2>(Vector<2> const&, RotMatrix<2> const&);

template Vector<3> operator* <3>(Vector<3> const&, RotMatrix<3> const&);

template Vector<2> operator* <2>(RotMatrix<2> const&, Vector<2> const&);

template Vector<3> operator* <3>(RotMatrix<3> const&, Vector<3> const&);

template Vector<2> ProdInv<2>(Vector<2> const&, RotMatrix<2> const&);

template Vector<3> ProdInv<3>(Vector<3> const&, RotMatrix<3> const&);

template Vector<3> Prod<3>(Vector<3> const&, RotMatrix<3> const&);

template Vector<2> Prod<2>(Vector<2> const&, RotMatrix<2> const&);

template RotMatrix<3> Prod<3>(RotMatrix<3> const&, RotMatrix<3> const&);

template RotMatrix<2> Prod<2>(RotMatrix<2> const&, RotMatrix<2> const&);

}
