/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2012 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Mitsuba 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, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/core/transform.h>
#include <mitsuba/core/frame.h>

MTS_NAMESPACE_BEGIN

// -----------------------------------------------------------------------
//  Linear transformation class
// -----------------------------------------------------------------------

Transform Transform::operator*(const Transform &t) const {
	return Transform(m_transform * t.m_transform,
		t.m_invTransform * m_invTransform);
}

Transform Transform::translate(const Vector &v) {
	Matrix4x4 trafo(
		1, 0, 0, v.x,
		0, 1, 0, v.y,
		0, 0, 1, v.z,
		0, 0, 0, 1
	);
	Matrix4x4 invTrafo(
		1, 0, 0, -v.x,
		0, 1, 0, -v.y,
		0, 0, 1, -v.z,
		0, 0, 0, 1
	);
	return Transform(trafo, invTrafo);
}

Transform Transform::scale(const Vector &v) {
	Matrix4x4 trafo(
		v.x, 0,   0,   0,
		0,   v.y, 0,   0,
		0,   0,   v.z, 0,
		0,   0,   0,   1
	);
	Matrix4x4 invTrafo(
		1.0f/v.x, 0,        0,        0,
		0,        1.0f/v.y, 0,        0,
		0,        0,        1.0f/v.z, 0,
		0,        0,        0,        1
	);
	return Transform(trafo, invTrafo);
}

Transform Transform::rotate(const Vector &axis, Float angle) {
	Float sinTheta, cosTheta;

	/* Make sure that the axis is normalized */
	Vector naxis = normalize(axis);
	math::sincos(degToRad(angle), &sinTheta, &cosTheta);

	Matrix4x4 result;
	result(0, 0) = naxis.x * naxis.x + (1.0f - naxis.x * naxis.x) * cosTheta;
	result(0, 1) = naxis.x * naxis.y * (1.0f - cosTheta) - naxis.z * sinTheta;
	result(0, 2) = naxis.x * naxis.z * (1.0f - cosTheta) + naxis.y * sinTheta;
	result(0, 3) = 0;

	result(1, 0) = naxis.x * naxis.y * (1.0f - cosTheta) + naxis.z * sinTheta;
	result(1, 1) = naxis.y * naxis.y + (1.0f - naxis.y * naxis.y) * cosTheta;
	result(1, 2) = naxis.y * naxis.z * (1.0f - cosTheta) - naxis.x * sinTheta;
	result(1, 3) = 0;

	result(2, 0) = naxis.x * naxis.z * (1.0f - cosTheta) - naxis.y * sinTheta;
	result(2, 1) = naxis.y * naxis.z * (1.0f - cosTheta) + naxis.x * sinTheta;
	result(2, 2) = naxis.z * naxis.z + (1.0f - naxis.z * naxis.z) * cosTheta;
	result(2, 3) = 0;

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

	/* The matrix is orthonormal */
	Matrix4x4 transp;
	result.transpose(transp);
	return Transform(result, transp);
}

Transform Transform::perspective(Float fov, Float clipNear, Float clipFar) {
	/* Project vectors in camera space onto a plane at z=1:
	 *
	 *  xProj = x / z
	 *  yProj = y / z
	 *  zProj = (far * (z - near)) / (z * (far-near))
	 *
	 *  Camera-space depths are not mapped linearly!
	 */
	Float recip = 1.0f / (clipFar - clipNear);

	/* Perform a scale so that the field of view is mapped
	 * to the interval [-1, 1] */
	Float cot = 1.0f / std::tan(degToRad(fov / 2.0f));

	Matrix4x4 trafo(
		cot,  0,    0,   0,
		0,    cot,  0,   0,
		0,    0,    clipFar * recip, -clipNear * clipFar * recip,
		0,    0,    1,   0
	);


	return Transform(trafo);
}

Transform Transform::glPerspective(Float fov, Float clipNear, Float clipFar) {
	Float recip = 1.0f / (clipNear - clipFar);
	Float cot = 1.0f / std::tan(degToRad(fov / 2.0f));

	Matrix4x4 trafo(
		cot,   0,     0,   0,
		0,     cot,   0,   0,
		0,     0,     (clipFar + clipNear) * recip,  2 * clipFar * clipNear * recip,
		0,     0,     -1,   0
	);

	return Transform(trafo);
}

Transform Transform::glFrustum(Float left, Float right, Float bottom, Float top, Float nearVal, Float farVal) {
	Float invFMN = 1 / (farVal-nearVal);
	Float invTMB = 1 / (top-bottom);
	Float invRML = 1 / (right-left);

	Matrix4x4 trafo(
		2*nearVal*invRML, 0, (right+left)*invRML, 0,
		0, 2*nearVal*invTMB, (top+bottom)*invTMB, 0,
		0, 0, -(farVal + nearVal) * invFMN, -2*farVal*nearVal*invFMN,
		0, 0, -1, 0
	);

	return Transform(trafo);
}

Transform Transform::orthographic(Float clipNear, Float clipFar) {
	return scale(Vector(1.0f, 1.0f, 1.0f / (clipFar - clipNear))) *
		   translate(Vector(0.0f, 0.0f, -clipNear));
}

Transform Transform::glOrthographic(Float clipNear, Float clipFar) {
	Float a = -2.0f / (clipFar - clipNear),
	      b = -(clipFar + clipNear) / (clipFar - clipNear);

	Matrix4x4 trafo(
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, a, b,
		0, 0, 0, 1
	);
	return Transform(trafo);
}

Transform Transform::glOrthographic(Float clipLeft, Float clipRight,
		Float clipBottom, Float clipTop, Float clipNear, Float clipFar) {
	Float fx =  2.0f / (clipRight - clipLeft),
	      fy =  2.0f / (clipTop - clipBottom),
	      fz = -2.0f / (clipFar - clipNear),
	      tx = -(clipRight + clipLeft) / (clipRight - clipLeft),
	      ty = -(clipTop + clipBottom) / (clipTop - clipBottom),
	      tz = -(clipFar + clipNear) / (clipFar - clipNear);

	Matrix4x4 trafo(
		fx,  0,  0,  tx,
		 0, fy,  0,  ty,
		 0,  0, fz,  tz,
		 0,  0,  0,   1
	);

	return Transform(trafo);
}

Transform Transform::lookAt(const Point &p, const Point &t, const Vector &up) {
	Vector dir = normalizeStrict(t-p, "lookAt(): 'origin' and 'target' coincide!");
	Vector left = normalizeStrict(cross(up, dir), "lookAt(): the forward and upward direction must be linearly independent!");
	Vector newUp = cross(dir, left);

	Matrix4x4 result, inverse;
	result(0, 0)  = left.x;  result(1, 0)  = left.y;  result(2, 0)  = left.z;  result(3, 0)  = 0;
	result(0, 1)  = newUp.x; result(1, 1)  = newUp.y; result(2, 1)  = newUp.z; result(3, 1)  = 0;
	result(0, 2)  = dir.x;   result(1, 2)  = dir.y;   result(2, 2)  = dir.z;   result(3, 2)  = 0;
	result(0, 3)  = p.x;     result(1, 3)  = p.y;     result(2, 3)  = p.z;     result(3, 3)  = 1;

	/* The inverse is simple to compute for this matrix, so do it directly here */
	Vector q(
		result(0, 0) * p.x + result(1, 0) * p.y + result(2, 0) * p.z,
		result(0, 1) * p.x + result(1, 1) * p.y + result(2, 1) * p.z,
		result(0, 2) * p.x + result(1, 2) * p.y + result(2, 2) * p.z);

	inverse(0, 0) = left.x; inverse(1, 0) = newUp.x; inverse(2, 0) = dir.x; inverse(3, 0) = 0;
	inverse(0, 1) = left.y; inverse(1, 1) = newUp.y; inverse(2, 1) = dir.y; inverse(3, 1) = 0;
	inverse(0, 2) = left.z; inverse(1, 2) = newUp.z; inverse(2, 2) = dir.z; inverse(3, 2) = 0;
	inverse(0, 3) = -q.x;   inverse(1, 3) = -q.y;    inverse(2, 3) = -q.z;  inverse(3, 3) = 1;

	return Transform(result, inverse);
}

Transform Transform::fromFrame(const Frame &frame) {
	Matrix4x4 result(
		frame.s.x, frame.t.x, frame.n.x, 0,
		frame.s.y, frame.t.y, frame.n.y, 0,
		frame.s.z, frame.t.z, frame.n.z, 0,
		0, 0, 0, 1
	);
	/* The matrix is orthonormal */
	Matrix4x4 transp;
	result.transpose(transp);
	return Transform(result, transp);
}

std::string Transform::toString() const {
	return m_transform.toString();
}

/**
 * \brief Tridiagonalizes a 3x3 matrix using a single
 * Householder transformation and returns the result.
 *
 * Based on "Geometric Tools" by David Eberly.
 */
static inline void tred3(Matrix3x3 &m, Float *diag, Float *subd) {
	Float m00 = m(0, 0), m01 = m(0, 1), m02 = m(0, 2),
	      m11 = m(1, 1), m12 = m(1, 2), m22 = m(2, 2);

	diag[0] = m00;
	subd[2] = 0;
	if (std::abs(m02) > std::numeric_limits<Float>::epsilon()) {
		Float length = std::sqrt(m01*m01 + m02*m02),
			  invLength = 1 / length;
		m01 *= invLength;
		m02 *= invLength;
		Float q = 2*m01*m12 + m02*(m22 - m11);
		diag[1] = m11 + m02*q;
		diag[2] = m22 - m02*q;
		subd[0] = length;
		subd[1] = m12 - m01*q;
		m = Matrix3x3(
			1, 0, 0,
			0, m01, m02,
			0, m02, -m01);
	} else {
		/* The matrix is already tridiagonal,
		   return an identity transformation matrix */
		diag[1] = m11;
		diag[2] = m22;
		subd[0] = m01;
		subd[1] = m12;
		m = Matrix3x3(
			1, 0, 0,
			0, 1, 0,
			0, 0, 1);
	}
}

/**
 * Compute the eigenvalues and eigenvectors of a 3x3 symmetric tridiagonal
 * matrix using the QL algorithm with implicit shifts
 *
 * Based on "Geometric Tools" by David Eberly and Numerical Recipes by
 * Teukolsky, et al. Originally, this code goes back to the Handbook
 * for Automatic Computation by Wilkionson and Reinsch, as well as
 * the corresponding EISPACK routine.
 */

static inline bool ql3(Matrix3x3 &m, Float *diag, Float *subd) {
	const int maxIter = 32;

	for (int i = 0; i < 3; ++i) {
		int k;
		for (k = 0; k < maxIter; ++k) {
			int j;
			for (j = i; j < 2; ++j) {
				Float tmp = std::abs(diag[j]) + std::abs(diag[j+1]);
				if (std::abs(subd[j]) + tmp == tmp)
					break;
			}
			if (j == i)
				break;

			Float value0 = (diag[i + 1] - diag[i])/(2*subd[i]);
			Float value1 = std::sqrt(value0*value0 + 1);
			value0 = diag[j] - diag[i] + subd[i] /
				((value0 < 0) ? (value0 - value1) : (value0 + value1));

			Float sn = 1, cs = 1, value2 = 0;
			for (int l = j - 1; l >= i; --l) {
				Float value3 = sn*subd[l], value4 = cs*subd[l];
				if (std::abs(value3) >= std::abs(value0)) {
					cs = value0 / value3;
					value1 = std::sqrt(cs*cs + 1);
					subd[l + 1] = value3*value1;
					sn = 1.0f / value1;
					cs *= sn;
				} else {
					sn = value3 / value0;
					value1 = std::sqrt(sn*sn + 1);
					subd[l + 1] = value0*value1;
					cs = 1.0f / value1;
					sn *= cs;
				}
				value0 = diag[l + 1] - value2;
				value1 = (diag[l] - value0)*sn + 2*value4*cs;
				value2 = sn*value1;
				diag[l + 1] = value0 + value2;
				value0 = cs*value1 - value4;

				for (int t = 0; t < 3; ++t) {
					value3 = m(t, l + 1);
					m(t, l + 1) = sn*m(t, l) + cs*value3;
					m(t, l) = cs*m(t, l) - sn*value3;
				}
			}
			diag[i] -= value2;
			subd[i] = value0;
			subd[j] = 0;
		}
		if (k == maxIter)
			return false;
	}
	return true;
}

/// Fast 3x3 eigenvalue decomposition
bool eig3(Matrix3x3 &m, Float lambda[3]) {
	Float subd[3];

	/* Reduce to Hessenberg form */
	tred3(m, lambda, subd);

	/* Iteratively find the eigenvalues and eigenvectors
	   using implicitly shifted QL iterations */
	if (!ql3(m, lambda, subd))
		return false;

	/* Sort the eigenvalues in decreasing order */
	for (int i=0; i<2; ++i) {
		/* Locate the maximum eigenvalue. */
		int largest = i;
		Float maxValue = lambda[largest];
		for (int j = i+1; j<3; ++j) {
			if (lambda[j] > maxValue) {
				largest = j;
				maxValue = lambda[largest];
			}
		}

		if (largest != i) {
			// Swap the eigenvalues.
			std::swap(lambda[i], lambda[largest]);

			// Swap the eigenvectors
			for (int j=0; j<3; ++j)
				std::swap(m(j, i), m(j, largest));
		}
	}

	return true;
}

/**
 * Compute the roots of the cubic polynomial.  Double-precision arithmetic
 * is used to minimize the effects due to subtractive cancellation.  The
 * roots are returned in increasing order.\
 *
 * Based on "Geometric Tools" by David Eberly.
 */
static void eig3_evals(const Matrix3x3 &A, double root[3]) {
	const double msInv3 = 1.0 / 3.0, msRoot3 = std::sqrt(3.0);

	// Convert the unique matrix entries to double precision.
	double a00 = (double) A(0, 0), a01 = (double) A(0, 1), a02 = (double) A(0, 2),
	       a11 = (double) A(1, 1), a12 = (double) A(1, 2), a22 = (double) A(2, 2);

	// The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
	// eigenvalues are the roots to this equation, all guaranteed to be
	// real-valued, because the matrix is symmetric.
	double c0 = a00*a11*a22 + 2.0*a01*a02*a12 - a00*a12*a12 -
		a11*a02*a02 - a22*a01*a01;

	double c1 = a00*a11 - a01*a01 + a00*a22 - a02*a02 +
		a11*a22 - a12*a12;

	double c2 = a00 + a11 + a22;

	// Construct the parameters used in classifying the roots of the equation
	// and in solving the equation for the roots in closed form.
	double c2Div3 = c2*msInv3;
	double aDiv3 = (c1 - c2*c2Div3)*msInv3;
	if (aDiv3 > 0.0)
		aDiv3 = 0.0;

	double halfMB = 0.5*(c0 + c2Div3*(2.0*c2Div3*c2Div3 - c1));

	double q = halfMB*halfMB + aDiv3*aDiv3*aDiv3;
	if (q > 0.0)
		q = 0.0;

	// Compute the eigenvalues by solving for the roots of the polynomial.
	double magnitude = std::sqrt(-aDiv3);
	double angle = std::atan2(std::sqrt(-q), halfMB)*msInv3;
	double cs = std::cos(angle);
	double sn = std::sin(angle);
	double root0 = c2Div3 + 2.0*magnitude*cs;
	double root1 = c2Div3 - magnitude*(cs + msRoot3*sn);
	double root2 = c2Div3 - magnitude*(cs - msRoot3*sn);

	// Sort in increasing order.
	if (root1 <= root0) {
		root[0] = root0;
		root[1] = root1;
	} else {
		root[0] = root1;
		root[1] = root0;
	}

	if (root2 <= root[1]) {
		root[2] = root2;
	} else {
		root[2] = root[1];
		if (root2 <= root[0]) {
			root[1] = root2;
		} else {
			root[1] = root[0];
			root[0] = root2;
		}
	}
}

/**
 * Determine if M has positive rank.  The maximum-magnitude entry of M is
 * returned.  The row in which it is contained is also returned.
 *
 * Based on "Geometric Tools" by David Eberly.
 */
static bool eig3_rank(const Matrix3x3 &m, Float& maxEntry, Vector &maxRow) {
    // Locate the maximum-magnitude entry of the matrix.
    maxEntry = -1.0f;
    int maxRowIndex = -1;
    for (int row = 0; row < 3; ++row) {
        for (int col = row; col < 3; ++col) {
            Float absValue = std::abs(m(row, col));
            if (absValue > maxEntry) {
                maxEntry = absValue;
                maxRowIndex = row;
            }
        }
    }

    // Return the row containing the maximum, to be used for eigenvector
    // construction.
    maxRow = m.row(maxRowIndex);

    return maxEntry >= std::numeric_limits<Float>::epsilon();
}

/**
 * Compute the eigenvectors
 *
 * Based on "Geometric Tools" by David Eberly.
 */
static void eig3_evecs(Matrix3x3& A, const Float lambda[3], const Vector& U2, int i0, int i1, int i2) {
	Vector U0, U1;
	coordinateSystem(normalize(U2), U0, U1);

	// V[i2] = c0*U0 + c1*U1,  c0^2 + c1^2=1
	// e2*V[i2] = c0*A*U0 + c1*A*U1
	// e2*c0 = c0*U0.Dot(A*U0) + c1*U0.Dot(A*U1) = d00*c0 + d01*c1
	// e2*c1 = c0*U1.Dot(A*U0) + c1*U1.Dot(A*U1) = d01*c0 + d11*c1
	Vector tmp = A*U0, evecs[3];

	Float p00 = lambda[i2] - dot(U0, tmp),
		  p01 = dot(U1, tmp),
		  p11 = lambda[i2] - dot(U1, A*U1),
		  maxValue = std::abs(p00),
		  absValue = std::abs(p01),
		  invLength;

	if (absValue > maxValue)
		maxValue = absValue;

	int row = 0;
	absValue = std::abs(p11);
	if (absValue > maxValue) {
		maxValue = absValue;
		row = 1;
	}

	if (maxValue >= std::numeric_limits<Float>::epsilon()) {
		if (row == 0) {
			invLength = 1/std::sqrt(p00*p00 + p01*p01);
			p00 *= invLength;
			p01 *= invLength;
			evecs[i2] = p01*U0 + p00*U1;
		} else {
			invLength = 1/std::sqrt(p11*p11 + p01*p01);
			p11 *= invLength;
			p01 *= invLength;
			evecs[i2] = p11*U0 + p01*U1;
		}
	} else {
		if (row == 0)
			evecs[i2] = U1;
		else
			evecs[i2] = U0;
	}

	// V[i0] = c0*U2 + c1*Cross(U2,V[i2]) = c0*R + c1*S
	// e0*V[i0] = c0*A*R + c1*A*S
	// e0*c0 = c0*R.Dot(A*R) + c1*R.Dot(A*S) = d00*c0 + d01*c1
	// e0*c1 = c0*S.Dot(A*R) + c1*S.Dot(A*S) = d01*c0 + d11*c1
	Vector S = cross(U2, evecs[i2]);
	tmp = A*U2;
	p00 = lambda[i0] - dot(U2, tmp);
	p01 = dot(S, tmp);
	p11 = lambda[i0] - dot(S, A*S);
	maxValue = std::abs(p00);
	row = 0;
	absValue = std::abs(p01);
	if (absValue > maxValue)
		maxValue = absValue;
	absValue = std::abs(p11);
	if (absValue > maxValue) {
		maxValue = absValue;
		row = 1;
	}

	if (maxValue >= std::numeric_limits<Float>::epsilon()) {
		if (row == 0) {
			invLength = 1/std::sqrt(p00*p00 + p01*p01);
			p00 *= invLength;
			p01 *= invLength;
			evecs[i0] = p01*U2 + p00*S;
		} else {
			invLength = 1/std::sqrt(p11*p11 + p01*p01);
			p11 *= invLength;
			p01 *= invLength;
			evecs[i0] = p11*U2 + p01*S;
		}
	} else {
		if (row == 0)
			evecs[i0] = S;
		else
			evecs[i0] = U2;
	}

	evecs[i1] = cross(evecs[i2], evecs[i0]);
	A = Matrix3x3(
		evecs[0].x, evecs[1].x, evecs[2].x,
		evecs[0].y, evecs[1].y, evecs[2].y,
		evecs[0].z, evecs[1].z, evecs[2].z
	);
}

/**
 * \brief Fast non-iterative 3x3 eigenvalue decomposition
 *
 * Based on "Geometric Tools" by David Eberly.
 */
void eig3_noniter(Matrix3x3 &A, Float lambda[3]) {
	// Compute the eigenvalues using double-precision arithmetic.
	double root[3];
	eig3_evals(A, root);
	lambda[0] = (Float) root[0];
	lambda[1] = (Float) root[1];
	lambda[2] = (Float) root[2];
	Matrix3x3 eigs;

	Float maxEntry[3];
	Vector maxRow[3];
	for (int i = 0; i < 3; ++i) {
		Matrix3x3 M(A);
		M(0, 0) -= lambda[i];
		M(1, 1) -= lambda[i];
		M(2, 2) -= lambda[i];
		if (!eig3_rank(M, maxEntry[i], maxRow[i])) {
			A.setIdentity();
			return;
		}
	}

	Float totalMax = maxEntry[0];
	int i = 0;
	if (maxEntry[1] > totalMax) {
		totalMax = maxEntry[1];
		i = 1;
	} if (maxEntry[2] > totalMax) {
		i = 2;
	}

	if (i == 0) {
		maxRow[0] = normalize(maxRow[0]);
		eig3_evecs(A, lambda, maxRow[0], 1, 2, 0);
	} else if (i == 1) {
		maxRow[1] = normalize(maxRow[1]);
		eig3_evecs(A, lambda, maxRow[1], 2, 0, 1);
	} else {
		maxRow[2] = normalize(maxRow[2]);
		eig3_evecs(A, lambda, maxRow[2], 0, 1, 2);
	}
}

MTS_NAMESPACE_END