#include "Matrix4x4.h"
#include "Vector3.h"

// standard headers
#include <iomanip>
using namespace std;

namespace GtpVisibilityPreprocessor {


Matrix4x4::Matrix4x4(const Vector3 &a, const Vector3 &b, const Vector3 &c)
{
  // first index is column [x], the second is row [y]
  x[0][0] = a.x;
  x[1][0] = b.x;
  x[2][0] = c.x;
  x[3][0] = 0.0f;

  x[0][1] = a.y;
  x[1][1] = b.y;
  x[2][1] = c.y;
  x[3][1] = 0.0f;

  x[0][2] = a.z;
  x[1][2] = b.z;
  x[2][2] = c.z;
  x[3][2] = 0.0f;

  x[0][3] = 0.0f;
  x[1][3] = 0.0f;
  x[2][3] = 0.0f;
  x[3][3] = 1.0f;

}

void
Matrix4x4::SetColumns(const Vector3 &a, const Vector3 &b, const Vector3 &c)
{
  // first index is column [x], the second is row [y]
  x[0][0] = a.x;
  x[1][0] = a.y;
  x[2][0] = a.z;
  x[3][0] = 0.0f;

  x[0][1] = b.x;
  x[1][1] = b.y;
  x[2][1] = b.z;
  x[3][1] = 0.0f;

  x[0][2] = c.x;
  x[1][2] = c.y;
  x[2][2] = c.z;
  x[3][2] = 0.0f;

  x[0][3] = 0.0f;
  x[1][3] = 0.0f;
  x[2][3] = 0.0f;
  x[3][3] = 1.0f;
}

// full constructor
Matrix4x4::Matrix4x4(float x11, float x12, float x13, float x14,
					 float x21, float x22, float x23, float x24,
					 float x31, float x32, float x33, float x34,
					 float x41, float x42, float x43, float x44)
{
  // first index is column [x], the second is row [y]
  x[0][0] = x11;
  x[1][0] = x12;
  x[2][0] = x13;
  x[3][0] = x14;

  x[0][1] = x21;
  x[1][1] = x22;
  x[2][1] = x23;
  x[3][1] = x24;

  x[0][2] = x31;
  x[1][2] = x32;
  x[2][2] = x33;
  x[3][2] = x34;

  x[0][3] = x41;
  x[1][3] = x42;
  x[2][3] = x43;
  x[3][3] = x44;
}

// inverse matrix computation gauss_jacobiho method .. from N.R. in C
// if matrix is regular = computatation successfull = returns 0
// in case of singular matrix returns 1
int
Matrix4x4::Invert()
{
  int indxc[4],indxr[4],ipiv[4];
  int i,icol,irow,j,k,l,ll,n;
  float big,dum,pivinv,temp;
  // satisfy the compiler
  icol = irow = 0;

  // the size of the matrix
  n = 4;

  for ( j = 0 ; j < n ; j++) /* zero pivots */
    ipiv[j] = 0;

  for ( i = 0; i < n; i++)
  {
    big = 0.0;
    for (j = 0 ; j < n ; j++)
      if (ipiv[j] != 1)
        for ( k = 0 ; k<n ; k++)
        {
          if (ipiv[k] == 0)
	  {
	    if (fabs(x[k][j]) >= big)
            {
	      big = fabs(x[k][j]);
              irow = j;
              icol = k;
            }
          }
          else
            if (ipiv[k] > 1)
	      return 1; /* singular matrix */
        }
    ++(ipiv[icol]);
    if (irow != icol)
    {
      for ( l = 0 ; l<n ; l++)
      {
        temp = x[l][icol];
        x[l][icol] = x[l][irow];
        x[l][irow] = temp;
      }
    }
    indxr[i] = irow;
    indxc[i] = icol;
    if (x[icol][icol] == 0.0)
      return 1; /* singular matrix */

    pivinv = 1.0 / x[icol][icol];
    x[icol][icol] = 1.0 ;
    for ( l = 0 ; l<n ; l++)
      x[l][icol] = x[l][icol] * pivinv ;
    
    for (ll = 0 ; ll < n ; ll++)
      if (ll != icol)
      {
        dum = x[icol][ll];
	x[icol][ll] = 0.0;
        for ( l = 0 ; l<n ; l++)
	  x[l][ll] = x[l][ll] - x[l][icol] * dum ;
      }
  }
  for ( l = n; l--; )
  {
    if (indxr[l] != indxc[l])
      for ( k = 0; k<n ; k++)
      {
        temp = x[indxr[l]][k];
	x[indxr[l]][k] = x[indxc[l]][k];
	x[indxc[l]][k] = temp;
      }
  }

  return 0 ; // matrix is regular .. inversion has been succesfull
}

// Invert the given matrix using the above inversion routine.
Matrix4x4
Invert(const Matrix4x4& M)
{
  Matrix4x4 InvertMe = M;
  InvertMe.Invert();
  return InvertMe;
}


// Transpose the matrix.
void
Matrix4x4::Transpose()
{
  for (int i = 0; i < 4; i++)
    for (int j = i; j < 4; j++)
      if (i != j) {
	float temp = x[i][j];
	x[i][j] = x[j][i];
	x[j][i] = temp;
      }
}

// Transpose the given matrix using the transpose routine above.
Matrix4x4
Transpose(const Matrix4x4& M)
{
  Matrix4x4 TransposeMe = M;
  TransposeMe.Transpose();
  return TransposeMe;
}

// Construct an identity matrix.
Matrix4x4
IdentityMatrix()
{
  Matrix4x4 M;

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      M.x[i][j] = (i == j) ? 1.0 : 0.0;
  return M;
}

// Construct a zero matrix.
Matrix4x4
ZeroMatrix()
{
  Matrix4x4 M;
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      M.x[i][j] = 0;
  return M;
}

// Construct a translation matrix given the location to translate to.
Matrix4x4
TranslationMatrix(const Vector3& Location)
{
  Matrix4x4 M = IdentityMatrix();
  M.x[3][0] = Location.x;
  M.x[3][1] = Location.y;
  M.x[3][2] = Location.z;
  return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// X axis.
Matrix4x4
RotationXMatrix(float Angle)
{
  Matrix4x4 M = IdentityMatrix();
  float Cosine = cos(Angle);
  float Sine = sin(Angle);
  M.x[1][1] = Cosine;
  M.x[2][1] = -Sine;
  M.x[1][2] = Sine;
  M.x[2][2] = Cosine;
  return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// Y axis.
Matrix4x4
RotationYMatrix(float Angle)
{
  Matrix4x4 M = IdentityMatrix();
  float Cosine = cos(Angle);
  float Sine = sin(Angle);
  M.x[0][0] = Cosine;
  M.x[2][0] = -Sine;
  M.x[0][2] = Sine;
  M.x[2][2] = Cosine;
  return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// Z axis.
Matrix4x4
RotationZMatrix(float Angle)
{
  Matrix4x4 M = IdentityMatrix();
  float Cosine = cos(Angle);
  float Sine = sin(Angle);
  M.x[0][0] = Cosine;
  M.x[1][0] = -Sine;
  M.x[0][1] = Sine;
  M.x[1][1] = Cosine;
  return M;
}

// Construct a yaw-pitch-roll rotation matrix.	Rotate Yaw
// radians about the XY axis, rotate Pitch radians in the
// plane defined by the Yaw rotation, and rotate Roll radians
// about the axis defined by the previous two angles.
Matrix4x4
RotationYPRMatrix(float Yaw, float Pitch, float Roll)
{
  Matrix4x4 M;
  float ch = cos(Yaw);
  float sh = sin(Yaw);
  float cp = cos(Pitch);
  float sp = sin(Pitch);
  float cr = cos(Roll);
  float sr = sin(Roll);

  M.x[0][0] = ch * cr + sh * sp * sr;
  M.x[1][0] = -ch * sr + sh * sp * cr;
  M.x[2][0] = sh * cp;
  M.x[0][1] = sr * cp;
  M.x[1][1] = cr * cp;
  M.x[2][1] = -sp;
  M.x[0][2] = -sh * cr - ch * sp * sr;
  M.x[1][2] = sr * sh + ch * sp * cr;
  M.x[2][2] = ch * cp;
  for (int i = 0; i < 4; i++)
    M.x[3][i] = M.x[i][3] = 0;
  M.x[3][3] = 1;

  return M;
}

// Construct a rotation of a given angle about a given axis.
// Derived from Eric Haines's SPD (Standard Procedural 
// Database).
Matrix4x4
RotationAxisMatrix(const Vector3& axis, float angle)
{
  Matrix4x4 M;
  double cosine = cos(angle);
  double sine = sin(angle);
  double one_minus_cosine = 1 - cosine;

  M.x[0][0] = axis.x * axis.x + (1.0 - axis.x * axis.x) * cosine;
  M.x[0][1] = axis.x * axis.y * one_minus_cosine + axis.z * sine;
  M.x[0][2] = axis.x * axis.z * one_minus_cosine - axis.y * sine;
  M.x[0][3] = 0;

  M.x[1][0] = axis.x * axis.y * one_minus_cosine - axis.z * sine;
  M.x[1][1] = axis.y * axis.y + (1.0 - axis.y * axis.y) * cosine;
  M.x[1][2] = axis.y * axis.z * one_minus_cosine + axis.x * sine;
  M.x[1][3] = 0;

  M.x[2][0] = axis.x * axis.z * one_minus_cosine + axis.y * sine;
  M.x[2][1] = axis.y * axis.z * one_minus_cosine - axis.x * sine;
  M.x[2][2] = axis.z * axis.z + (1.0 - axis.z * axis.z) * cosine;
  M.x[2][3] = 0;

  M.x[3][0] = 0;
  M.x[3][1] = 0;
  M.x[3][2] = 0;
  M.x[3][3] = 1;

  return M;
}


// Constructs the rotation matrix that rotates 'vec1' to 'vec2'
Matrix4x4
RotationVectorsMatrix(const Vector3 &vecStart,
		      const Vector3 &vecTo)
{
  Vector3 vec = CrossProd(vecStart, vecTo);
  
  if (Magnitude(vec) > Limits::Small) {
    // vector exist, compute angle
    float angle = acos(DotProd(vecStart, vecTo));
    // normalize for sure
    vec.Normalize();
    return RotationAxisMatrix(vec, angle);
  }

  // opposite or colinear vectors
  Matrix4x4 ret = IdentityMatrix();
  if (DotProd(vecStart, vecTo) < 0.0)
    ret *= -1.0; // opposite vectors

  return ret;
}


// Construct a scale matrix given the X, Y, and Z parameters
// to scale by.  To scale uniformly, let X==Y==Z.
Matrix4x4
ScaleMatrix(float X, float Y, float Z)
{
  Matrix4x4 M = IdentityMatrix();

  M.x[0][0] = X;
  M.x[1][1] = Y;
  M.x[2][2] = Z;

  return M;
}

// Construct a rotation matrix that makes the x, y, z axes
// correspond to the vectors given.
Matrix4x4
GenRotation(const Vector3 &x, const Vector3 &y, const Vector3 &z)
{
  Matrix4x4 M = IdentityMatrix();

#if 1
  //  x  y
  M.x[0][0] = x.x;
  M.x[1][0] = x.y;
  M.x[2][0] = x.z;

  M.x[0][1] = y.x;
  M.x[1][1] = y.y;
  M.x[2][1] = y.z;

  M.x[0][2] = z.x;
  M.x[1][2] = z.y;
  M.x[2][2] = z.z;
#else
  //  x  y -- old version
  M.x[0][0] = x.x;
  M.x[0][1] = x.y;
  M.x[0][2] = x.z;

  M.x[1][0] = y.x;
  M.x[1][1] = y.y;
  M.x[1][2] = y.z;

  M.x[2][0] = z.x;
  M.x[2][1] = z.y;
  M.x[2][2] = z.z;
#endif

  return M;
}

// Construct a quadric matrix.	After Foley et al. pp. 528-529.
Matrix4x4
QuadricMatrix(float a, float b, float c, float d, float e,
	      float f, float g, float h, float j, float k)
{
  Matrix4x4 M;

  M.x[0][0] = a;  M.x[0][1] = d;  M.x[0][2] = f;  M.x[0][3] = g;
  M.x[1][0] = d;  M.x[1][1] = b;  M.x[1][2] = e;  M.x[1][3] = h;
  M.x[2][0] = f;  M.x[2][1] = e;  M.x[2][2] = c;  M.x[2][3] = j;
  M.x[3][0] = g;  M.x[3][1] = h;  M.x[3][2] = j;  M.x[3][3] = k;

  return M;
}

// Construct various "mirror" matrices, which flip coordinate
// signs in the various axes specified.
Matrix4x4
MirrorX()
{
  Matrix4x4 M = IdentityMatrix();
  M.x[0][0] = -1;
  return M;
}

Matrix4x4
MirrorY()
{
  Matrix4x4 M = IdentityMatrix();
  M.x[1][1] = -1;
  return M;
}

Matrix4x4
MirrorZ()
{
  Matrix4x4 M = IdentityMatrix();
  M.x[2][2] = -1;
  return M;
}

Matrix4x4
RotationOnly(const Matrix4x4& x)
{
  Matrix4x4 M = x;
  M.x[3][0] = M.x[3][1] = M.x[3][2] = 0;
  return M;
}

// Add corresponding elements of the two matrices.
Matrix4x4&
Matrix4x4::operator+= (const Matrix4x4& A)
{
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      x[i][j] += A.x[i][j];
  return *this;
}

// Subtract corresponding elements of the matrices.
Matrix4x4&
Matrix4x4::operator-= (const Matrix4x4& A)
{
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      x[i][j] -= A.x[i][j];
  return *this;
}

// Scale each element of the matrix by A.
Matrix4x4&
Matrix4x4::operator*= (float A)
{
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      x[i][j] *= A;
  return *this;
}

// Multiply two matrices.
Matrix4x4&
Matrix4x4::operator*= (const Matrix4x4& A)
{
  Matrix4x4 ret = *this;

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++) {
      float subt = 0;
      for (int k = 0; k < 4; k++)
	subt += ret.x[i][k] * A.x[k][j];
      x[i][j] = subt;
    }
  return *this;
}

// Add corresponding elements of the matrices.
Matrix4x4
operator+ (const Matrix4x4& A, const Matrix4x4& B)
{
  Matrix4x4 ret;

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      ret.x[i][j] = A.x[i][j] + B.x[i][j];
  return ret;
}

// Subtract corresponding elements of the matrices.
Matrix4x4
operator- (const Matrix4x4& A, const Matrix4x4& B)
{
  Matrix4x4 ret;

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      ret.x[i][j] = A.x[i][j] - B.x[i][j];
  return ret;
}

// Multiply matrices.
Matrix4x4
operator* (const Matrix4x4& A, const Matrix4x4& B)
{
  Matrix4x4 ret;

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++) {
      float subt = 0;
      for (int k = 0; k < 4; k++)
	subt += A.x[i][k] * B.x[k][j];
      ret.x[i][j] = subt;
    }
  return ret;
}

// Transform a vector by a matrix.
Vector3
operator* (const Matrix4x4& M, const Vector3& v)
{
  Vector3 ret;
  float denom;

  ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0] + M.x[3][0];
  ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1] + M.x[3][1];
  ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2] + M.x[3][2];
  denom = M.x[0][3] + M.x[1][3] + M.x[2][3] + M.x[3][3];
  if (denom != 1.0)
    ret /= denom;
  return ret;
}

// Apply the rotation portion of a matrix to a vector.
Vector3
RotateOnly(const Matrix4x4& M, const Vector3& v)
{
  Vector3 ret;
  float denom;

  ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0];
  ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1];
  ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2];
  denom = M.x[0][3] + M.x[1][3] + M.x[2][3] + M.x[3][3];
  if (denom != 1.0)
    ret /= denom;
  return ret;
}

// Scale each element of the matrix by B.
Matrix4x4
operator* (const Matrix4x4& A, float B)
{
  Matrix4x4 ret;

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      ret.x[i][j] = A.x[i][j] * B;
  return ret;
}

// Overloaded << for C++-style output.
ostream&
operator<< (ostream& s, const Matrix4x4& M)
{
  for (int i = 0; i < 4; i++) { // y
    for (int j = 0; j < 4; j++) { // x
                                          //  x  y 
      s << setprecision(4) << setw(10) << M.x[j][i];
    }
    s << '\n';
  }
  return s;
}

// Rotate a direction vector...
Vector3
PlaneRotate(const Matrix4x4& tform, const Vector3& p)
{
  // I sure hope that matrix is invertible...
  Matrix4x4 use = Transpose(Invert(tform));

  return RotateOnly(use, p);
}

// Transform a normal
Vector3
TransformNormal(const Matrix4x4& tform, const Vector3& n)
{
  Matrix4x4 use = NormalTransformMatrix(tform);

  return RotateOnly(use, n);
}
 
Matrix4x4 
NormalTransformMatrix(const Matrix4x4 &tform)
{
  Matrix4x4 m = tform;
  // for normal translation vector must be zero!
  m.x[3][0] = m.x[3][1] = m.x[3][2] = 0.0; 
  // I sure hope that matrix is invertible...
  return Transpose(Invert(m));
}

Vector3
GetTranslation(const Matrix4x4 &M)
{
  return Vector3(M.x[3][0], M.x[3][1], M.x[3][2]);
}

}