#pragma once 
#include <stdio.h>
#include <math.h>
#include "point3D.h"
#include "vector3D.h"
#include "matrix4x4.h"

matrix4x4::matrix4x4()
{
	/* constructor */
	initMatrixIdentity();
	PI = 3.14159265358979323846f;
}
matrix4x4::matrix4x4(float a, float b, float c, float d,
					 float e, float f, float g, float h,
					 float i, float j, float k, float l,
					 float m, float n, float o, float p)
{
	/* constructor  from 16 values*/
	matrix[0][0] = a;
	matrix[0][1] = b;
	matrix[0][2] = c;
	matrix[0][3] = d;
	matrix[1][0] = e;
	matrix[1][1] = f;
	matrix[1][2] = g;
	matrix[1][3] = h;
	matrix[2][0] = i;
	matrix[2][1] = j;
	matrix[2][2] = k;
	matrix[2][3] = l;
	matrix[3][0] = m;
	matrix[3][1] = n;
	matrix[3][2] = o;
	matrix[3][3] = p;
	PI = 3.14159265358979323846f;
}
void matrix4x4::writeMatrix(int row, int col, float value)
{
	matrix[row][col] = value;
}
float matrix4x4::readMatrix(int row, int col)
{
	return (matrix[row][col]);
}
void matrix4x4::initMatrix(float value)
{
	int i,j;
	for(i=0; i<4; i++)
	{
		for(j=0; j<4; j++)
		{
			matrix[i][j] = value;
		}
	}
}	
void matrix4x4::initMatrixIdentity()
{
	int i,j;
	for(i=0; i<4; i++)
	{
		for(j=0; j<4; j++)
		{
			if(i==j)
			{
				matrix[i][j] = 1;
			}else{
				matrix[i][j] = 0;
			}
		}
	}
	return;
}
void matrix4x4::initMatrixXYZRotation(float x, float y, float z)
{
        matrix4x4 mx( 1, 0, 0, 0,
					  0, cos(x), -sin(x), 0,
					  0, sin(x), cos(x), 0,
					  0,0,0,1);
        matrix4x4 my( cos(y), 0, sin(y), 0,
					  0, 1, 0, 0,
					  -sin(y), 0, cos(y),0,
					  0,0,0,1);
        matrix4x4 mz( cos(z), -sin(z), 0, 0,
					  sin(z), cos(z), 0, 0,
					  0, 0, 1,0,
					  0,0,0,1);
        matrix4x4 temp =  mx * my * mz;

		matrix[0][0] = temp.readMatrix(0,0);
		matrix[0][1] = temp.readMatrix(0,1);
		matrix[0][2] = temp.readMatrix(0,2);
		matrix[0][3] = temp.readMatrix(0,3);
		matrix[1][0] = temp.readMatrix(1,0);
		matrix[1][1] = temp.readMatrix(1,1);
		matrix[1][2] = temp.readMatrix(1,2);
		matrix[1][3] = temp.readMatrix(1,3);
		matrix[2][0] = temp.readMatrix(2,0);
		matrix[2][1] = temp.readMatrix(2,1);
		matrix[2][2] = temp.readMatrix(2,2);
		matrix[2][3] = temp.readMatrix(2,3);
		matrix[3][0] = temp.readMatrix(3,0);
		matrix[3][1] = temp.readMatrix(3,1);
		matrix[3][2] = temp.readMatrix(3,2);
		matrix[3][3] = temp.readMatrix(3,3);
}
void matrix4x4::initMatrixTranslation(float x, float y, float z){
	/* 1 0 0 x
	   0 1 0 y
	   0 0 1 z
	   0 0 0 0 */
	initMatrixIdentity();
	matrix[0][3] = x;
	matrix[1][3] = y;
	matrix[2][3] = z;
	matrix[3][3] = 0;
	return;
}
void matrix4x4::initMatrixUniformScale(vector3D x){
	/* sx 0 0 0 
	   0 sy 0 0
	   0 0 sz 0
	   0 0  0 1 */
	initMatrixIdentity();
	matrix[0][0] *= x[0];
	matrix[1][1] *= x[1];
	matrix[2][2] *= x[2];
	return;
}
void matrix4x4::initMatrixPerspective(float f){
	/* f 0 0 0 
	   0 f 0 0 
	   0 0 0 1
	   0 0 1 0 */
	initMatrix(0);
	matrix[0][0] = f;
	matrix[1][1] = f;
	matrix[3][2] = 1;
	matrix[2][3] = 1;
	return;
}

matrix4x4 matrix4x4::transposeMatrix()
{
	matrix4x4 tempMatrix;
	int i,j;
	for(i=0; i<4; i++)
	{
		for(j=0; j<4; j++)
		{
			//tempMatrix[i][j] = matrix[j][i];
			tempMatrix.writeMatrix(i,j,matrix[j][i]);
		}
	}
	return tempMatrix;
}
matrix4x4 matrix4x4::operator*(matrix4x4 m2)
{
	matrix4x4 result;
	int i,j,k;
	for(i=0; i<4; i++){
		for(j=0; j<4; j++){
			//result[i][j] = 0;
			result.writeMatrix(i,j,0.0);
			for(k=0; k<4; k++){
				//result[i][j] = result[i][j] + matrix[i][k]*m2[k][j];
				result.writeMatrix(i,j, result.readMatrix(i,j) + matrix[i][k]*m2.readMatrix(k,j));
			}
		}
	}
	return result;
}
point3D matrix4x4::operator*(point3D x)
{
	float PADVALUE = 1.0;
	float t[3];
	t[0] = 0;
	t[1] = 0;
	t[2] = 0;
	// pad POINTS with 1, VECTORS with 0
	// TODO: check if POINT X = [x1,x2,x3,1] or [x1,x2,x3,0]
	int i,j;

	// multiply first 3 rows
	for(i=0; i<3; i++){
		for(j=0; j<3; j++){
			t[i] += matrix[i][j] * x[j];
		}
		// multiply by 4th row of column vector (PADVALUE)
		t[i] += matrix[i][3] * PADVALUE;
	}
	// multiply 4th row, store result in temp float
	float fourth = 0;
	for(j=0; j<3; j++)
	{
		fourth += matrix[3][j] * x[j];
	}
	fourth += matrix[3][3] * PADVALUE;

	// Divide all elements in result by 4th row value
	if(fourth != 0)
	{
		for(i=0; i<3; i++)
		{
			t[i] /= fourth;
		}
	}
	point3D result(t[0], t[1], t[2]);
	return result;
}
vector3D matrix4x4::operator*(vector3D x)
{
	float PADVALUE = 0.0;
	float t[3];
	t[0] = 0;
	t[1] = 0;
	t[2] = 0;
	// pad POINTS with 1, VECTORS with 0
	// TODO: check if POINT X = [x1,x2,x3,1] or [x1,x2,x3,0]
	int i,j;

	// multiply first 3 rows
	for(i=0; i<3; i++){
		for(j=0; j<3; j++){
			t[i] += matrix[i][j] * x[j];
		}
		// multiply by 4th row of column vector (PADVALUE)
		t[i] += matrix[i][3] * PADVALUE;
	}
	// multiply 4th row, store result in temp float
	float fourth = 0;
	for(j=0; j<3; j++)
	{
		fourth += matrix[3][j] * x[j];
	}
	fourth += matrix[3][3] * PADVALUE;

	// Divide all elements in result by 4th row value
	if(fourth != 0)
	{
		for(i=0; i<3; i++)
		{
			t[i] /= fourth;
		}
	}
	vector3D result(t[0], t[1], t[2]);
	return result;
}