#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <string.h>

#include "rtsVector3d.h"
#include "rtsPoint3d.h"

using namespace std;
#ifndef RTSMATRIX4X4_H
#define RTSMATRIX4X4_H

#define RTS_PI	3.14159

///This class represents a 4x4 matrix, which is often used to represent affine transformations on 3D points and vectors.

///
///This class is designed to work with point3d<T> and vector3d<T>.  Data is stored internally in a column-major form which is compatible with OpenGL.
///

template <class T>
class matrix4x4
{
	/*stored as:
	| 0  4  8  12 |
    |             |
    | 1  5  9  13 |
    |             |
    | 2  6  10 14 |
    |             |
    | 3  7  11 15 |
	*/
public:
	T m_matrix[16];

	//constructors
	matrix4x4();								///<Constructor initializes all values to zero.
	matrix4x4(T c0r0, T c0r1, T c0r2, T c0r3, 
			  T c1r0, T c1r1, T c1r2, T c1r3, 
			  T c2r0, T c2r1, T c2r2, T c2r3, 
			  T c3r0, T c3r1, T c3r2, T c3r3);	///<Constructor initializes all values to those specified as parameters. c=column, r=row
	
	//overloaded operators
	T& operator()(int row, int column){return m_matrix[column*4 + row];}	///<Returns the value at the specified position.  This function can be used as both m(0, 0) = x and x = m(0, 0)
	vector3D<T> operator*(vector3D<T> rhs);		///<Overloaded arithmetic operator.  This function multiplies a vector by the current matrix, returning the transformed vector3D.
	point3D<T> operator*(point3D<T> rhs);		///<Overloaded arithmetic operator.  This function multiplies a point by the current matrix, returning the transformed point3D.
	matrix4x4<T> operator*(matrix4x4<T> rhs);	///<Overloaded arithmetic operator.  Computes the result of a matrix x matrix multiplication.

	//methods
	void SetIdentity();							///<Sets the current matrix to the identity matrix.
	void SetRotation(T angle, T x, T y, T z);	///<Creates a matrix that represents a rotation of "angle" degrees around the axis given by (x, y, z).
	void SetTranslation(T x, T y, T z);			///<Creates a matrix that represents a translation specified by the vector (x, y, z).
	void SetScale(T x, T y, T z);				///<Creates a matrix that represents a scale along each axis given by the scalars x, y, and z.

	//output
	void Print(int width = 7, int precision = 2);///<Sends the current matrix to the standard output.

};


template <class T>
matrix4x4<T>::matrix4x4()
{
	memset(m_matrix, 0, sizeof(T)*16);
}

template <class T>
matrix4x4<T>::matrix4x4(T c0r0, T c0r1, T c0r2, T c0r3, 
						T c1r0, T c1r1, T c1r2, T c1r3, 
						T c2r0, T c2r1, T c2r2, T c2r3, 
						T c3r0, T c3r1, T c3r2, T c3r3)
{

	T new_matrix[16] = {c0r0,c0r1,c0r2,c0r3,c1r0,c1r1,c1r2,c1r3,c2r0,c2r1,c2r2,c2r3,c3r0,c3r1,c3r2,c3r3};
	memcpy(m_matrix, new_matrix, 16*sizeof(T));
}

template <class T>
vector3D<T> matrix4x4<T>::operator*(vector3D<T> rhs)
{
	vector3D<T> result;
	result.x = m_matrix[0] * rhs.x + m_matrix[4] * rhs.y + m_matrix[8] * rhs.z;
	result.y = m_matrix[1] * rhs.x + m_matrix[5] * rhs.y + m_matrix[9] * rhs.z;
	result.z = m_matrix[2] * rhs.x + m_matrix[6] * rhs.y + m_matrix[10] * rhs.z;
	return result;
}

template <class T>
point3D<T> matrix4x4<T>::operator *(point3D<T> rhs)
{
	point3D<T> result;
	result.x = m_matrix[0] * rhs.x + m_matrix[4] * rhs.y + m_matrix[8] * rhs.z + m_matrix[12];
	result.y = m_matrix[1] * rhs.x + m_matrix[5] * rhs.y + m_matrix[9] * rhs.z + m_matrix[13];
	result.z = m_matrix[2] * rhs.x + m_matrix[6] * rhs.y + m_matrix[10] * rhs.z + m_matrix[14];
	T w = m_matrix[3] + m_matrix[7] + m_matrix[11] + m_matrix[15];
	result.x/=w;
	result.y/=w;
	result.z/=w;
	return result;
}

template <class T>
matrix4x4<T> matrix4x4<T>::operator *(matrix4x4<T> rhs)
{
	matrix4x4<T> result;
	int i,j,r;
	for(i = 0; i<4; i++)
		for(j = 0; j<4; j++)
			for(r=0; r<4; r++)
				result(i, j) += (*this)(i, r) * rhs(r, j);

	return result;
}




template <class T>
void matrix4x4<T>::SetIdentity()
{
	T new_matrix[16] = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
	memcpy(m_matrix, new_matrix, 16*sizeof(T));
}

template <class T>
void matrix4x4<T>::SetTranslation(T x, T y, T z)
{
	T new_matrix[16] = {1,0,0,0,0,1,0,0,0,0,1,0,x,y,z,1};
	memcpy(m_matrix, new_matrix, 16*sizeof(T));
}

template <class T>
void matrix4x4<T>::SetScale(T x, T y, T z)
{
	T new_matrix[16] = {x,0,0,0,0,y,0,0,0,0,z,0,0,0,0,1};
	memcpy(m_matrix, new_matrix, 16*sizeof(T));
}

template <class T>
void matrix4x4<T>::SetRotation(T angle, T x, T y, T z)
{
	//create the axis of rotation
	vector3D<T> axis(x, y, z);
	T length = axis.Length();	//make sure that it is normalized
	if(length != 1)
		axis.Normalize();

	T c = cos(angle*RTS_PI/180.0);			//compute the sine and cosine of angle
	T s = sin(angle*RTS_PI/180.0);

	//create the matrix
	m_matrix[0] = x*x*(1-c) + c;
	m_matrix[1] = y*x*(1-c) + z*s;
	m_matrix[2] = x*z*(1-c) - y*s;
	m_matrix[4] = x*y*(1-c) - z*s;
	m_matrix[5] = y*y*(1-c) + c;
	m_matrix[6] = y*z*(1-c) + x*s;
	m_matrix[8] = x*z*(1-c) + y*s;
	m_matrix[9] = y*z*(1-c) - x*s;
	m_matrix[10]= z*z*(1-c) + c;
	m_matrix[15]= 1;
}


template <class T>
void matrix4x4<T>::Print(int width, int precision)
{
	cout<<setiosflags(ios::fixed);
	cout<<setprecision(precision);
	cout<<setw(width)<<m_matrix[0]
		<<setw(width)<<m_matrix[4]
		<<setw(width)<<m_matrix[8]
		<<setw(width)<<m_matrix[12]<<endl
		<<setw(width)<<m_matrix[1]
		<<setw(width)<<m_matrix[5]
		<<setw(width)<<m_matrix[9]
		<<setw(width)<<m_matrix[13]<<endl
		<<setw(width)<<m_matrix[2]
		<<setw(width)<<m_matrix[6]
		<<setw(width)<<m_matrix[10]
		<<setw(width)<<m_matrix[14]<<endl
		<<setw(width)<<m_matrix[3]
		<<setw(width)<<m_matrix[7]
		<<setw(width)<<m_matrix[11]
		<<setw(width)<<m_matrix[15]<<endl;
}

	
	






#endif