////////////////////////////////////////////////////////////////////////////////
// Matrix.cpp
// Implementation of matrix operations
// Copyright Björn Ganster
// Developed since 2001
////////////////////////////////////////////////////////////////////////////////

#include <math.h>
#include <assert.h>

#include "Matrix.h"

#include "GL/glu.h"

#ifndef gluInvertMatrix
bool gluInvertMatrix(const double m[16], double invOut[16])
{
    double inv[16], det;
    int i;

    inv[0] = m[5]  * m[10] * m[15] - 
             m[5]  * m[11] * m[14] - 
             m[9]  * m[6]  * m[15] + 
             m[9]  * m[7]  * m[14] +
             m[13] * m[6]  * m[11] - 
             m[13] * m[7]  * m[10];

    inv[4] = -m[4]  * m[10] * m[15] + 
              m[4]  * m[11] * m[14] + 
              m[8]  * m[6]  * m[15] - 
              m[8]  * m[7]  * m[14] - 
              m[12] * m[6]  * m[11] + 
              m[12] * m[7]  * m[10];

    inv[8] = m[4]  * m[9] * m[15] - 
             m[4]  * m[11] * m[13] - 
             m[8]  * m[5] * m[15] + 
             m[8]  * m[7] * m[13] + 
             m[12] * m[5] * m[11] - 
             m[12] * m[7] * m[9];

    inv[12] = -m[4]  * m[9] * m[14] + 
               m[4]  * m[10] * m[13] +
               m[8]  * m[5] * m[14] - 
               m[8]  * m[6] * m[13] - 
               m[12] * m[5] * m[10] + 
               m[12] * m[6] * m[9];

    inv[1] = -m[1]  * m[10] * m[15] + 
              m[1]  * m[11] * m[14] + 
              m[9]  * m[2] * m[15] - 
              m[9]  * m[3] * m[14] - 
              m[13] * m[2] * m[11] + 
              m[13] * m[3] * m[10];

    inv[5] = m[0]  * m[10] * m[15] - 
             m[0]  * m[11] * m[14] - 
             m[8]  * m[2] * m[15] + 
             m[8]  * m[3] * m[14] + 
             m[12] * m[2] * m[11] - 
             m[12] * m[3] * m[10];

    inv[9] = -m[0]  * m[9] * m[15] + 
              m[0]  * m[11] * m[13] + 
              m[8]  * m[1] * m[15] - 
              m[8]  * m[3] * m[13] - 
              m[12] * m[1] * m[11] + 
              m[12] * m[3] * m[9];

    inv[13] = m[0]  * m[9] * m[14] - 
              m[0]  * m[10] * m[13] - 
              m[8]  * m[1] * m[14] + 
              m[8]  * m[2] * m[13] + 
              m[12] * m[1] * m[10] - 
              m[12] * m[2] * m[9];

    inv[2] = m[1]  * m[6] * m[15] - 
             m[1]  * m[7] * m[14] - 
             m[5]  * m[2] * m[15] + 
             m[5]  * m[3] * m[14] + 
             m[13] * m[2] * m[7] - 
             m[13] * m[3] * m[6];

    inv[6] = -m[0]  * m[6] * m[15] + 
              m[0]  * m[7] * m[14] + 
              m[4]  * m[2] * m[15] - 
              m[4]  * m[3] * m[14] - 
              m[12] * m[2] * m[7] + 
              m[12] * m[3] * m[6];

    inv[10] = m[0]  * m[5] * m[15] - 
              m[0]  * m[7] * m[13] - 
              m[4]  * m[1] * m[15] + 
              m[4]  * m[3] * m[13] + 
              m[12] * m[1] * m[7] - 
              m[12] * m[3] * m[5];

    inv[14] = -m[0]  * m[5] * m[14] + 
               m[0]  * m[6] * m[13] + 
               m[4]  * m[1] * m[14] - 
               m[4]  * m[2] * m[13] - 
               m[12] * m[1] * m[6] + 
               m[12] * m[2] * m[5];

    inv[3] = -m[1] * m[6] * m[11] + 
              m[1] * m[7] * m[10] + 
              m[5] * m[2] * m[11] - 
              m[5] * m[3] * m[10] - 
              m[9] * m[2] * m[7] + 
              m[9] * m[3] * m[6];

    inv[7] = m[0] * m[6] * m[11] - 
             m[0] * m[7] * m[10] - 
             m[4] * m[2] * m[11] + 
             m[4] * m[3] * m[10] + 
             m[8] * m[2] * m[7] - 
             m[8] * m[3] * m[6];

    inv[11] = -m[0] * m[5] * m[11] + 
               m[0] * m[7] * m[9] + 
               m[4] * m[1] * m[11] - 
               m[4] * m[3] * m[9] - 
               m[8] * m[1] * m[7] + 
               m[8] * m[3] * m[5];

    inv[15] = m[0] * m[5] * m[10] - 
              m[0] * m[6] * m[9] - 
              m[4] * m[1] * m[10] + 
              m[4] * m[2] * m[9] + 
              m[8] * m[1] * m[6] - 
              m[8] * m[2] * m[5];

    det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

    if (det == 0)
        return false;

    det = 1.0 / det;

    for (i = 0; i < 16; i++)
        invOut[i] = inv[i] * det;

    return true;
}
#endif

////////////////////////////////////////////////////////////////////////////////
// Construct unit matrix

Matrix::Matrix ()
: VirtualClass ("Matrix", MatrixID)
{
   for (unsigned int i = 0; i < 16; i++)
      if (i % 4 == i / 4)
         m[i] = 1.0; // let diagonals contain 1
      else 
         m[i] = 0.0; // let other entries contain 0
}

////////////////////////////////////////////////////////////////////////////////
// Construct matrix from vectors

Matrix::Matrix(const Point3D& v1, const Point3D& v2, const Point3D& v3)
: VirtualClass ("Matrix", MatrixID)
{
	m[0] = v1.x; m[1] = v1.y; m[ 2] = v1.z; m[3] = 0.0;
	m[4] = v2.x; m[5] = v2.y; m[ 6] = v2.z; m[7] = 0.0;
	m[8] = v3.x; m[9] = v3.y; m[10] = v3.z; m[11] = 0.0;
	m[12] = 0.0; m[13] = 0.0; m[14] =  0.0; m[15] = 1.0;

	// todo: need to calculate the inverse matrix
	//inverse[0] =  ???
	gluInvertMatrix(m, inverse);
}

////////////////////////////////////////////////////////////////////////////////
// Apply matrix to a point

Point3D Matrix::Transform (const Point3D& p) const
{
   return Point3D (p.x*m[ 0] + p.y*m[ 4] + p.z*m[ 8] + p.w*m[12],
                   p.x*m[ 1] + p.y*m[ 5] + p.z*m[ 9] + p.w*m[13],
                   p.x*m[ 2] + p.y*m[ 6] + p.z*m[10] + p.w*m[14],
                   p.x*m[ 3] + p.y*m[ 7] + p.z*m[11] + p.w*m[15]);
}

////////////////////////////////////////////////////////////////////////////////
// Matrix multiplication

Matrix Matrix::operator* (const Matrix& b) const
{
	// Looped version
	Matrix res;
	for (unsigned int x = 0; x < 4; x++) {
		for (unsigned int y = 0; y < 4; y++) {
			res.m[x*4+y] = 0.0;
			for (unsigned int z = 0; z < 4; z++)
				res.m[x*4+y] += m[z*4+y]*b.m[x*4+z];
		}
	}

	for (unsigned int x = 0; x < 4; x++) {
		for (unsigned int y = 0; y < 4; y++) {
			res.inverse[x*4+y] = 0.0;
			for (unsigned int z = 0; z < 4; z++)
				res.inverse[x*4+y] += inverse[z*4+y]*b.inverse[x*4+z];
		}
	}

	return res;
}

////////////////////////////////////////////////////////////////////////////////
// Assignment operators

void Matrix::operator = (const Matrix& M)
{
	for (unsigned int i = 0; i < 16; i++) {
		m[i] = M.m[i];
		inverse[i] = M.inverse[i];
	}
}

// Assignment operator with double array
// If you write d as an array with 4 values per line, then: "In den Spalten stehen die Bilder der Basisvektoren"
void Matrix::operator = (const double* d)
{
	for (unsigned int i = 0; i < 16; i++)
		m[i] = d[i];

	// todo: need to calculate the inverse matrix
	gluInvertMatrix(m, inverse);
}

////////////////////////////////////////////////////////////////////////////////
// Return the transpose of a matrix, useful e.g. to calculate transforms on normals

Matrix Matrix::transpose() const
{
	Matrix result;
	for (unsigned int i = 0; i < 4; ++i) {
		for (unsigned int j = 0; j < 4; ++j) {
			auto val = m[4*i+j];
			result.m[4*j+i] = val;
			val = inverse[4*i+j];
			result.inverse[4*j+i] = val;
		}
	}
	return result;
}

////////////////////////////////////////////////////////////////////////////////
// Get render matrix stack (singleton)

MatrixStack* renderMatrixStack = 0;

MatrixStack* MatrixStack::getRenderMatrixStack ()
{
   if (renderMatrixStack == NULL) {
      renderMatrixStack = new MatrixStack();
   }

   return renderMatrixStack;
}

////////////////////////////////////////////////////////////////////////////////
// Load a translation matrix

TranslationMatrix::TranslationMatrix (double x, double y, double z)
{
   m[ 0] = 1.0; m[ 4] = 0.0; m[ 8] = 0.0; m[12] = x;
   m[ 1] = 0.0; m[ 5] = 1.0; m[ 9] = 0.0; m[13] = y;
   m[ 2] = 0.0; m[ 6] = 0.0; m[10] = 1.0; m[14] = z;
   m[ 3] = 0.0; m[ 7] = 0.0; m[11] = 0.0; m[15] = 1.0;

   inverse[0] = 1.0; inverse[4] = 0.0; inverse[ 8] = 0.0; inverse[12] = -x;
   inverse[1] = 0.0; inverse[5] = 1.0; inverse[ 9] = 0.0; inverse[13] = -y;
   inverse[2] = 0.0; inverse[6] = 0.0; inverse[10] = 1.0; inverse[14] = -z;
   inverse[3] = 0.0; inverse[7] = 0.0; inverse[11] = 0.0; inverse[13] = 1.0;
}

TranslationMatrix::TranslationMatrix (const Point3D& p)
{
   m[ 0] = 1.0; m[ 4] = 0.0; m[ 8] = 0.0; m[12] = p.x;
   m[ 1] = 0.0; m[ 5] = 1.0; m[ 9] = 0.0; m[13] = p.y;
   m[ 2] = 0.0; m[ 6] = 0.0; m[10] = 1.0; m[14] = p.z;
   m[ 3] = 0.0; m[ 7] = 0.0; m[11] = 0.0; m[15] = 1.0;

   inverse[0] = 1.0; inverse[4] = 0.0; inverse[ 8] = 0.0; inverse[12] = -p.x;
   inverse[1] = 0.0; inverse[5] = 1.0; inverse[ 9] = 0.0; inverse[13] = -p.y;
   inverse[2] = 0.0; inverse[6] = 0.0; inverse[10] = 1.0; inverse[14] = -p.z;
   inverse[3] = 0.0; inverse[7] = 0.0; inverse[11] = 0.0; inverse[13] = 1.0;
}

////////////////////////////////////////////////////////////////////////////////
// Load scaling matrix

ScaleMatrix::ScaleMatrix (double sx, double sy, double sz, double sw)
{
	m[ 0] =  sx; m[ 3] = 0.0; m[ 8] = 0.0; m[12] = 0.0;
	m[ 1] = 0.0; m[ 5] =  sy; m[ 9] = 0.0; m[13] = 0.0;
	m[ 2] = 0.0; m[ 6] = 0.0; m[10] =  sz; m[14] = 0.0;
	m[ 3] = 0.0; m[ 7] = 0.0; m[11] = 0.0; m[15] =  sw;

	inverse[0] = 1.0/sx; inverse[4] = 0.0; inverse[ 8] = 0.0; inverse[12] = 0.0;
	inverse[1] = 0.0; inverse[5] = 1.0/sy; inverse[ 9] = 0.0; inverse[13] = 0.0;
	inverse[2] = 0.0; inverse[6] = 0.0; inverse[10] = 1.0/sz; inverse[14] = 0.0;
	inverse[3] = 0.0; inverse[7] = 0.0; inverse[11] = 0.0;    inverse[13] = 1.0;
}

////////////////////////////////////////////////////////////////////////////////
// Load perspective projection matrix

GLUPerspectiveMatrix::GLUPerspectiveMatrix (double nearClippingPlane, 
	double farClippingPlane, double tanFOVY)
{
   // This is how it's done in GLU
   // Let's assume aspect to be 1.0, meaning that width == height
   // (This makes sense for HemiPlane/HemiCube pictures, because the x/y axes
   // are more or less arbitrary)
   double deltaZ = farClippingPlane - nearClippingPlane;
   double cotan = 1.0 / tanFOVY;

   m[ 0] = cotan; // cotan / aspect;
   m[ 5] = cotan;
   m[10] = -(farClippingPlane + nearClippingPlane) / deltaZ;
   m[11] = -1.0;
   m[14] = -2.0 * nearClippingPlane * farClippingPlane / deltaZ;
   m[15] = 0.0;
}

////////////////////////////////////////////////////////////////////////////////
// Load rotation matrix 
// See Graphics Gems by Andrew S. Glassner, p. 466

RotationMatrix::RotationMatrix (double angle, double x, double y, double z)
{
	double s = (double) sin(angle);
	double c = (double) cos(angle);
	double t = 1 - c;

	m[ 0] = t*x*x+c;   m[ 4] = t*x*y+s*z; m[ 8] = t*x*z-s*y; m[12] = 0.0;
	m[ 1] = t*x*y-s*z; m[ 5] = t*y*y+c;   m[ 9] = t*y*z+s*x; m[13] = 0.0;
	m[ 2] = t*x*z+s*y; m[ 6] = t*y*z-s*x; m[10] = t*z*z+c;   m[14] = 0.0;
	m[ 3] = 0.0;       m[ 7] = 0.0;       m[11] = 0.0;       m[15] = 1.0;

	s = (double) sin(-angle);
	c = (double) cos(-angle);
	t = 1 - c;

	inverse[ 0] = t*x*x+c;   inverse[ 4] = t*x*y+s*z; inverse[ 8] = t*x*z-s*y; inverse[12] = 0.0;
	inverse[ 1] = t*x*y-s*z; inverse[ 5] = t*y*y+c;   inverse[ 9] = t*y*z+s*x; inverse[13] = 0.0;
	inverse[ 2] = t*x*z+s*y; inverse[ 6] = t*y*z-s*x; inverse[10] = t*z*z+c;   inverse[14] = 0.0;
	inverse[ 3] = 0.0;       inverse[ 7] = 0.0;       inverse[11] = 0.0;       inverse[15] = 1.0;
}

////////////////////////////////////////////////////////////////////////////////
// LookDirs
// ViewingDir, UpDir are assumed to be unit length and orthogonal to each other

/*LookDirsMatrix::LookDirsMatrix (const Point3D& CameraPos, 
                    const Point3D& ViewingDir, 
                    const Point3D& UpDir)
{
   // We need a third vector, orthogonal to ViewingDir and UpDir
   Point3D side = ViewingDir.crossMultiply (UpDir);
   side.unitLength ();

   // We specify the transpose of the following matrix:
   // (1, 0, 0) becomes side,
   // (0, 1, 0) becomes UpDir,
   // (0, 0, 1) becomes ViewingDir
   // Since this matrix contains vectors that are orthogonal and that have
   // unit length, the matrix is orthonormal. Therefore, the transpose matrix
   // is the inverse matrix of the one described above.
   Matrix R; // rotational matrix
   R.m[ 0] = side.x;        R.m[ 4] = side.y;        R.m[ 8] = side.z;
   R.m[ 1] = UpDir.x;       R.m[ 5] = UpDir.y;       R.m[ 9] = UpDir.z;
   R.m[ 2] = -ViewingDir.x; R.m[ 6] = -ViewingDir.y; R.m[10] = -ViewingDir.z;

   // Translate into CameraPos
   TranslationMatrix T (-CameraPos);
   ((Matrix*) this)->operator= (R*T);
}

////////////////////////////////////////////////////////////////////////////////
// New version with ViewCenter

GluLookAtMatrix::GluLookAtMatrix (const Point3D& CameraPos, 
                                  const Point3D& ViewCenter, 
                                  const Point3D& UpDir)
{
   // We need a third vector, orthogonal to ViewingDir and UpDir
   Point3D dist = ViewCenter - CameraPos;
   Point3D distU = dist;
   distU.unitLength();
   Point3D UpDirU = UpDir;
   UpDirU.unitLength();
   Point3D side = distU.crossMultiply (UpDirU);
   UpDirU = side.crossMultiply (distU);

   // We specify the transpose of the following matrix:
   // (1, 0, 0) becomes side,
   // (0, 1, 0) becomes UpDir,
   // (0, 0, 1) becomes ViewingDir
   // Since this matrix contains vectors that are orthogonal and that have
   // unit length, the matrix is orthonormal. Therefore, the transpose matrix
   // is the inverse matrix of the one described above.
   Matrix R; // rotational matrix
   R.m[ 0] = side.x;   R.m[ 4] = side.y;   R.m[ 8] = side.z;
   R.m[ 1] = UpDirU.x; R.m[ 5] = UpDirU.y; R.m[ 9] = UpDirU.z;
   R.m[ 2] = -distU.x; R.m[ 6] = -distU.y; R.m[10] = -distU.z;

   // Translate into CameraPos
   TranslationMatrix T (-CameraPos);
   ((Matrix*) this)->operator= (R*T);
}*/

////////////////////////////////////////////////////////////////////////////////
// Load a GL matrix

MatrixFromOpenGL ::MatrixFromOpenGL (GLenum matrix)
{
	glGetDoublev (matrix, m);
}

// Return the inverted matrix
Matrix Matrix::inverted() const
{
	Matrix M;
	for (int i = 0; i < 16; ++i) {
		M.m[i] = inverse[i];
		M.inverse[i] = m[i];
	}
	return M;
}
