#include <vector>
#include "Mat4.h"
#include <math.h>
using namespace std;


class ModelViewMatrix{
public:
	vector<Mat4> stack; //top of stack is on the left (opposite of what you normally think)
	vector<Mat4> inverseStack;
	Mat4 getIdentityMatrix(){
		Mat4 m;
		for(int i = 0; i < 4; i++){
			for(int j = 0; j < 4; j++){
				if(i == j)
					m.m[i][j] = 1;
				else
					m.m[i][j] = 0;
			}
		}
		return m;
	}

	Mat4 transpose(Mat4 m){
		Mat4 a;
		for(int i = 0; i<4; i++)
			for(int j = 0; j<4; j++)
				a.m[i][j] = m.m[j][i];
		return a;
	}

	void init(){
		Mat4 m = getIdentityMatrix();
		stack.clear();
		stack.push_back(m);
		inverseStack.clear();
		inverseStack.push_back(m);
	}

	// --------------------------
    Mat3 m4_submat( Mat4 mr, int i, int j )
      {
		  Mat3 mb;
      int ti, tj, idst, jdst;

      for ( ti = 0; ti < 4; ti++ )
        {
        if ( ti < i )
          idst = ti;
        else
          if ( ti > i )
            idst = ti-1;

        for ( tj = 0; tj < 4; tj++ )
          {
          if ( tj < j )
            jdst = tj;
          else
            if ( tj > j )
              jdst = tj-1;

          if ( ti != i && tj != j )
            mb.m[(idst*3 + jdst)] = mr.m[(ti*4 + tj)/4 ][(ti*4 + tj)%4];
          }
        }
	  return mb;
      }

  //  --------------------------

 // The determinant of a 4x4 matrix can be calculated as follows:

  // --------------------------

    float m3_det( Mat3 mat )
      {
      float det;

      det = mat.m[0] * ( mat.m[4]*mat.m[8] - mat.m[7]*mat.m[5] )
          - mat.m[1] * ( mat.m[3]*mat.m[8] - mat.m[6]*mat.m[5] )
          + mat.m[2] * ( mat.m[3]*mat.m[7] - mat.m[6]*mat.m[4] );

      return det;
      }

    float m4_det( Mat4 mr ){
      float  det, result = 0, i = 1;
      Mat3 msub3;
      int     n;

      for ( n = 0; n < 4; n++, i *= -1 ){
		msub3 = m4_submat( mr, 0, n );
        det     = m3_det( msub3 );
        result += mr.m[n/4][n%4] * det * i;
        }

      return result;
      }

   // --------------------------

 // And the inverse can be calculated as follows:

   // --------------------------

    Mat4 m4_inverse(Mat4 ma )
      {
		  Mat4 mr;
      float  mdet = m4_det( ma );
      Mat3 mtemp;
      int     i, j, sign;

	  if ( fabs( mdet ) < 0.0005 ){
		  fprintf(stderr, "inverse problem");
        exit(1);
	  }

      for ( i = 0; i < 4; i++ )
        for ( j = 0; j < 4; j++ )
          {
          sign = 1 - ( (i +j) % 2 ) * 2;

          mtemp = m4_submat( ma, i, j );

          mr.m[(i+j*4)/4][(i+j*4)%4] = ( m3_det( mtemp ) * sign ) / mdet;
          }

      return mr;
      }

    //--------------------------
	void translate(double x, double y, double z){
		Mat4 m = getIdentityMatrix();
		m.m[0][3] = x;
		m.m[1][3] = y;
		m.m[2][3] = z;
		int last = stack.size() - 1;
		stack[last] = multiply(m,stack[last]);

		Mat4 mi = getIdentityMatrix();
		mi.m[0][3] = -x;
		mi.m[1][3] = -y;
		mi.m[2][3] = -z;
		inverseStack[last] = multiply(inverseStack[last],mi);
	}
	void rotate(double theta, double x, double y, double z){
		double pi = 3.14159265;
		theta = (theta/180.0)*pi;

		Mat4 newSpace = getIdentityMatrix();
		//our x y and z values define an arbitrary axis that we want to rotate around
		//first we need to set up a transformation into this space, find orthogonal axis
		vec3 newX = vec3(x, y, z);	
		newX.normalize();
		vec3 newY;
		if(newX.dotProduct(vec3(0, 1, 0)) < 1 )
			newY = newX.crossProduct(vec3(0, 1, 0));
		else
			newY = newX.crossProduct(vec3(1, 0, 0));
		vec3 newZ = newX.crossProduct(newY);

		newSpace.m[0][0] = newX.x;
		newSpace.m[0][1] = newX.y;
		newSpace.m[0][2] = newX.z;
		newSpace.m[1][0] = newY.x;
		newSpace.m[1][1] = newY.y;
		newSpace.m[1][2] = newY.z;
		newSpace.m[2][0] = newZ.x;
		newSpace.m[2][1] = newZ.y;
		newSpace.m[2][2] = newZ.z;

	/*	
		newSpace.m[0][0] = newX.x;
		newSpace.m[1][0] = newX.y;
		newSpace.m[2][0] = newX.z;
		newSpace.m[0][1] = newY.x;
		newSpace.m[1][1] = newY.y;
		newSpace.m[2][1] = newY.z;
		newSpace.m[0][2] = newZ.x;
		newSpace.m[1][2] = newZ.y;
		newSpace.m[2][2] = newZ.z;
	*/	
	//	newSpace = getIdentityMatrix();
		Mat4 rotation = getIdentityMatrix();
		rotation.m[0][0] = cos(theta);
		rotation.m[0][2] = sin(theta);
		rotation.m[2][0] = -sin(theta);
		rotation.m[2][2] = cos(theta);
		rotation = multiply(rotation, newSpace);
		int last = stack.size() - 1;
		stack[last] = multiply(rotation,stack[last]);
		
		//set up the inverse rotation matrix
		Mat4 inverseRotation = getIdentityMatrix();
		inverseRotation.m[0][0] = cos(-theta);
		inverseRotation.m[0][2] = sin(-theta);
		inverseRotation.m[2][0] = -sin(-theta);
		inverseRotation.m[2][2] = cos(-theta);
		inverseRotation = multiply(newSpace,inverseRotation);
		
		inverseStack[last] = multiply(inverseStack[last],inverseRotation);

	}
	void scale(double x, double y, double z){
		Mat4 m = getIdentityMatrix();
		m.m[0][0] = x;
		m.m[1][1] = y;
		m.m[2][2] = z;
		int last = stack.size() - 1;
		stack[last] = multiply(m,stack[last]);

		Mat4 mi = getIdentityMatrix();
		mi.m[0][0] = 1.0/x;
		mi.m[1][1] = 1.0/y;
		mi.m[2][2] = 1.0/z;
		inverseStack[last] = multiply(inverseStack[last],mi);

	}
	void pushMatrix(){
		stack.push_back(getIdentityMatrix());
		inverseStack.push_back(getIdentityMatrix());
	}
	void popMatrix(){
		stack.pop_back();
		inverseStack.pop_back();
	}
	Mat4 matrix(){
		int last = stack.size() - 1;
		return stack[last];
	}
	Mat4 inverseMatrix(){
		
		int last = stack.size() - 1;
		//return inverseStack[last];
		return m4_inverse(stack[last]);
	}
};