/*
	QGL Transform
	Ivan Pelipenko peri4ko@yandex.ru

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as published by
	the Free Software Foundation, either version 3 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 Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/


#include "gltransform.h"
#include "gltypes.h"
#include <cmath>


inline void composeQMatrix4x4(const QVector3D & position, const QVector3D & orientation, const QVector3D & scale, QMatrix4x4 &m) {
	const QMatrix3x3 rot3x3(Transform::toRotationMatrix(orientation));

	// set up final matrix with scale, rotation and translation
	m(0, 0) = scale.x() * rot3x3(0, 0); m(0, 1) = scale.y() * rot3x3(0, 1); m(0, 2) = scale.z() * rot3x3(0, 2); m(0, 3) = position.x();
	m(1, 0) = scale.x() * rot3x3(1, 0); m(1, 1) = scale.y() * rot3x3(1, 1); m(1, 2) = scale.z() * rot3x3(1, 2); m(1, 3) = position.y();
	m(2, 0) = scale.x() * rot3x3(2, 0); m(2, 1) = scale.y() * rot3x3(2, 1); m(2, 2) = scale.z() * rot3x3(2, 2); m(2, 3) = position.z();
	// no projection term
	m(3, 0) = 0.0f; m(3, 1) = 0.0f; m(3, 2) = 0.0f; m(3, 3) = 1.0f;
}

inline void decomposeQMatrix3x3(const QMatrix3x3 & m, QMatrix3x3 & Q, QVector3D & D, QVector3D & U) {
	// Factor M = QR = QDU where Q is orthogonal, D is diagonal,
	// and U is upper triangular with ones on its diagonal.
	// Algorithm uses Gram-Schmidt orthogonalization (the QR algorithm).
	//
	// If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
	//   q0 = m0/|m0|
	//   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
	//   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
	//
	// where |V| indicates length of vector V and A*B indicates dot
	// product of vectors A and B.  The matrix R has entries
	//
	//   r00 = q0*m0  r01 = q0*m1  r02 = q0*m2
	//   r10 = 0      r11 = q1*m1  r12 = q1*m2
	//   r20 = 0      r21 = 0      r22 = q2*m2
	//
	// so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
	// u02 = r02/r00, and u12 = r12/r11.

	// Q = rotation
	// D = scaling
	// U = shear

	// D stores the three diagonal entries r00, r11, r22
	// U stores the entries U[0] = u01, U[1] = u02, U[2] = u12

	// build orthogonal matrix Q
	float invLen = 1.0f / std::sqrt(m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0) + m(2, 0) * m(2, 0));
	Q(0, 0) = m(0, 0) * invLen;
	Q(1, 0) = m(1, 0) * invLen;
	Q(2, 0) = m(2, 0) * invLen;

	float dot = Q(0, 0) * m(0, 1) + Q(1, 0) * m(1, 1) + Q(2, 0) * m(2, 1);
	Q(0, 1) = m(0, 1) - dot * Q(0, 0);
	Q(1, 1) = m(1, 1) - dot * Q(1, 0);
	Q(2, 1) = m(2, 1) - dot * Q(2, 0);
	invLen = 1.0f / std::sqrt(Q(0, 1) * Q(0, 1) + Q(1, 1) * Q(1, 1) + Q(2, 1) * Q(2, 1));
	Q(0, 1) *= invLen;
	Q(1, 1) *= invLen;
	Q(2, 1) *= invLen;

	dot = Q(0, 0) * m(0, 2) + Q(1, 0) * m(1, 2) + Q(2, 0) * m(2, 2);
	Q(0, 2) = m(0, 2) - dot * Q(0, 0);
	Q(1, 2) = m(1, 2) - dot * Q(1, 0);
	Q(2, 2) = m(2, 2) - dot * Q(2, 0);
	dot = Q(0, 1) * m(0, 2) + Q(1, 1) * m(1, 2) + Q(2, 1) * m(2, 2);
	Q(0, 2) -= dot * Q(0, 1);
	Q(1, 2) -= dot * Q(1, 1);
	Q(2, 2) -= dot * Q(2, 1);
	invLen = 1.0f / std::sqrt(Q(0, 2) * Q(0, 2) + Q(1, 2) * Q(1, 2) + Q(2, 2) * Q(2, 2));
	Q(0, 2) *= invLen;
	Q(1, 2) *= invLen;
	Q(2, 2) *= invLen;

	// guarantee that orthogonal matrix has determinant 1 (no reflections)
	const float det = Q(0, 0) * Q(1, 1) * Q(2, 2) + Q(0, 1) * Q(1, 2) * Q(2, 0) +
					  Q(0, 2) * Q(1, 0) * Q(2, 1) - Q(0, 2) * Q(1, 1) * Q(2, 0) -
					  Q(0, 1) * Q(1, 0) * Q(2, 2) - Q(0, 0) * Q(1, 2) * Q(2, 1);
	if (det < 0.0f)
		Q *= -1.0f;

	// build "right" matrix R
	QMatrix3x3 R(Qt::Uninitialized);
	R(0, 0) = Q(0, 0) * m(0, 0) + Q(1, 0) * m(1, 0) + Q(2, 0) * m(2, 0);
	R(0, 1) = Q(0, 0) * m(0, 1) + Q(1, 0) * m(1, 1) + Q(2, 0) * m(2, 1);
	R(1, 1) = Q(0, 1) * m(0, 1) + Q(1, 1) * m(1, 1) + Q(2, 1) * m(2, 1);
	R(0, 2) = Q(0, 0) * m(0, 2) + Q(1, 0) * m(1, 2) + Q(2, 0) * m(2, 2);
	R(1, 2) = Q(0, 1) * m(0, 2) + Q(1, 1) * m(1, 2) + Q(2, 1) * m(2, 2);
	R(2, 2) = Q(0, 2) * m(0, 2) + Q(1, 2) * m(1, 2) + Q(2, 2) * m(2, 2);

	// the scaling component
	D[0] = R(0, 0);
	D[1] = R(1, 1);
	D[2] = R(2, 2);

	// the shear component
	U[0] = R(0, 1) / D[0];
	U[1] = R(0, 2) / D[0];
	U[2] = R(1, 2) / D[1];
}

inline bool hasScale(const QMatrix4x4 &m) {
	// If the columns are orthonormal and form a right-handed system, then there is no scale
	float t(m.determinant());
	if (!qFuzzyIsNull(t - 1.0f))
		return true;
	t = m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0) + m(2, 0) * m(2, 0);
	if (!qFuzzyIsNull(t - 1.0f))
		return true;
	t = m(0, 1) * m(0, 1) + m(1, 1) * m(1, 1) + m(2, 1) * m(2, 1);
	if (!qFuzzyIsNull(t - 1.0f))
		return true;
	t = m(0, 2) * m(0, 2) + m(1, 2) * m(1, 2) + m(2, 2) * m(2, 2);
	if (!qFuzzyIsNull(t - 1.0f))
		return true;
	return false;
}

inline void decomposeQMatrix4x4(const QMatrix4x4 & m, QVector3D & position, QVector3D & orientation, QVector3D & scale) {
	Q_ASSERT(m.isAffine());

	const QMatrix3x3 m3x3(m.toGenericMatrix<3, 3>());

	QMatrix3x3 rot3x3(Qt::Uninitialized);
	if (hasScale(m)) {
		decomposeQMatrix3x3(m3x3, rot3x3, scale, position);
	} else {
		// we know there is no scaling part; no need for QDU decomposition
		scale = QVector3D(1.0f, 1.0f, 1.0f);
		rot3x3 = m3x3;
	}
	orientation = Transform::fromRotationMatrix(rot3x3);
	position = QVector3D(m(0, 3), m(1, 3), m(2, 3));
}




Transform::Transform(): m_scale(1.0f, 1.0f, 1.0f),
	m_translation(), m_eulerRotationAngles(), m_matrixDirty(false) {
}


Transform & Transform::operator =(const Transform & t) {
	m_scale = t.m_scale;
	m_translation = t.m_translation;
	m_eulerRotationAngles = t.m_eulerRotationAngles;
	m_matrixDirty = true;
	return *this;
}


void Transform::setMatrix(const QMatrix4x4 & m) {
	if (m != matrix()) {
		m_matrix = m;
		m_matrixDirty = false;
		QVector3D s;
		QVector3D t;
		QVector3D r;
		decomposeQMatrix4x4(m, t, r, s);
		m_scale = s;
		m_translation = t;
		m_eulerRotationAngles = r;
	}
}


void Transform::setRotationX(float r) {
	if (m_eulerRotationAngles.x() == r)
		return;
	m_eulerRotationAngles.setX(r);
	m_matrixDirty = true;
}


void Transform::setRotationY(float r) {
	if (m_eulerRotationAngles.y() == r)
		return;
	m_eulerRotationAngles.setY(r);
	m_matrixDirty = true;
}


void Transform::setRotationZ(float r) {
	if (m_eulerRotationAngles.z() == r)
		return;
	m_eulerRotationAngles.setZ(r);
	m_matrixDirty = true;
}


QMatrix4x4 Transform::matrix() const {
	buildMatrix();
	return m_matrix;
}


QMatrix4x4 Transform::matrixRotate() const {
	buildMatrix();
	return m_matrixR;
}


QMatrix4x4 Transform::matrixScale() const {
	buildMatrix();
	return m_matrixS;
}


QMatrix4x4 Transform::matrixRotateScale() const {
	buildMatrix();
	return m_matrixWT;
}


QVector3D Transform::direction() const {
	return matrixRotate().mapVector(QVector3D(0,0,-1)).normalized();
}


void Transform::buildMatrix() const {
	if (m_matrixDirty) {
		composeQMatrix4x4(m_translation, m_eulerRotationAngles, m_scale, m_matrix);
		composeQMatrix4x4(QVector3D(), m_eulerRotationAngles, m_scale, m_matrixWT);
		composeQMatrix4x4(QVector3D(), m_eulerRotationAngles, QVector3D(1,1,1), m_matrixR);
		composeQMatrix4x4(QVector3D(), QVector3D(), m_scale, m_matrixS);
		m_matrixDirty = false;
	}
}


float Transform::rotationX() const {
	return m_eulerRotationAngles.x();
}


float Transform::rotationY() const {
	return m_eulerRotationAngles.y();
}


float Transform::rotationZ() const {
	return m_eulerRotationAngles.z();
}


void Transform::setScale(const QVector3D & s) {
	if (s != m_scale) {
		m_scale = s;
		m_matrixDirty = true;
	}
}


void Transform::setScaleX(float s) {
	if (s != m_scale.x()) {
		m_scale.setX(s);
		m_matrixDirty = true;
	}
}


void Transform::setScaleY(float s) {
	if (s != m_scale.y()) {
		m_scale.setY(s);
		m_matrixDirty = true;
	}
}


void Transform::setScaleZ(float s) {
	if (s != m_scale.z()) {
		m_scale.setZ(s);
		m_matrixDirty = true;
	}
}


QVector3D Transform::scale3D() const {
	return m_scale;
}


float Transform::scale() const {
	return m_scale.x();
}


void Transform::setRotation(const QVector3D & r) {
	if (r != m_eulerRotationAngles) {
		m_eulerRotationAngles = r;
		m_matrixDirty = true;
	}
}


QVector3D Transform::rotation() const {
	return m_eulerRotationAngles;
}


void Transform::setTranslation(const QVector3D & t) {
	if (t != m_translation) {
		m_translation = t;
		m_matrixDirty = true;
	}
}


void Transform::setTranslationX(float t) {
	if (t != m_translation.x()) {
		m_translation.setX(t);
		m_matrixDirty = true;
	}
}


void Transform::setTranslationY(float t) {
	if (t != m_translation.y()) {
		m_translation.setY(t);
		m_matrixDirty = true;
	}
}


void Transform::setTranslationZ(float t) {
	if (t != m_translation.z()) {
		m_translation.setZ(t);
		m_matrixDirty = true;
	}
}


QVector3D Transform::translation() const {
	return m_translation;
}


QQuaternion Transform::fromAxisAndAngle(const QVector3D & axis, float angle) {
	return QQuaternion::fromAxisAndAngle(axis, angle);
}


QQuaternion Transform::fromAxisAndAngle(float x, float y, float z, float angle) {
	return QQuaternion::fromAxisAndAngle(x, y, z, angle);
}


QQuaternion Transform::fromAxesAndAngles(const QVector3D & axis1, float angle1,
										 const QVector3D & axis2, float angle2) {
	const QQuaternion q1 = QQuaternion::fromAxisAndAngle(axis1, angle1);
	const QQuaternion q2 = QQuaternion::fromAxisAndAngle(axis2, angle2);
	return q2 * q1;
}


QQuaternion Transform::fromAxesAndAngles(const QVector3D & axis1, float angle1,
										 const QVector3D & axis2, float angle2,
										 const QVector3D & axis3, float angle3) {
	const QQuaternion q1 = QQuaternion::fromAxisAndAngle(axis1, angle1);
	const QQuaternion q2 = QQuaternion::fromAxisAndAngle(axis2, angle2);
	const QQuaternion q3 = QQuaternion::fromAxisAndAngle(axis3, angle3);
	return q3 * q2 * q1;
}


QQuaternion Transform::fromAxes(const QVector3D & xAxis, const QVector3D & yAxis, const QVector3D & zAxis) {
	return QQuaternion::fromAxes(xAxis, yAxis, zAxis);
}


QVector3D Transform::fromDirection(QVector3D d, float pitch) {
	QVector3D ret;
	d.normalize();
	ret[0] = M_PI - acos(d.z());
	ret[1] = pitch * deg2rad;
	ret[2] = -atan2(d.x(), d.y());
	normalizeAngleRad(ret[0]);
	normalizeAngleRad(ret[2]);
	return ret * rad2deg;
}


QVector3D Transform::fromRotationMatrix(const QMatrix3x3 & m) {
	//return QQuaternion::fromRotationMatrix(m);
	float sy = sqrt(m(0,0) * m(0,0) + m(1,0) * m(1,0));
	bool singular = sy < 1.E-6; // If
	float x, y, z;
	if (!singular) {
		x = atan2( m(2,1), m(2,2));
		y = atan2(-m(2,0), sy);
		z = atan2( m(1,0), m(0,0));
	} else {
		x = atan2(-m(1,2), m(1,1));
		y = atan2(-m(2,0), sy);
		z = 0.;
	}
	return QVector3D(x, y, z) * rad2deg;
}


QMatrix3x3 Transform::toRotationMatrix(const QVector3D & r) {
	QMatrix4x4 m;
	if (r.z() != 0.f) m.rotate(r.z(), 0., 0., 1.);
	if (r.y() != 0.f) m.rotate(r.y(), 0., 1., 0.);
	if (r.x() != 0.f) m.rotate(r.x(), 1., 0., 0.);
	return m.toGenericMatrix<3,3>();
}


QMatrix4x4 Transform::rotateAround(const QVector3D & point, float angle, const QVector3D & axis) {
	QMatrix4x4 m;
	m.translate(point);
	m.rotate(angle, axis);
	m.translate(-point);
	return m;
}


QMatrix4x4 Transform::rotateFromAxes(const QVector3D & xAxis, const QVector3D & yAxis, const QVector3D & zAxis) {
	return QMatrix4x4(xAxis.x(), yAxis.x(), zAxis.x(), 0.0f,
					  xAxis.y(), yAxis.y(), zAxis.y(), 0.0f,
					  xAxis.z(), yAxis.z(), zAxis.z(), 0.0f,
					  0.0f, 0.0f, 0.0f, 1.0f);
}