//All OpenGL matrizes found at: http://pyopengl.sourceforge.net/documentation/manual/

#include "MatrixHelper.h"
#include "arsMath.h"
#include "Vector3D.h"
#include <math.h>
//#include <stdio.h>
#include <cstdio>


float CoTan(float a){
	return 1 / tan(a * M_PI / 180); 
}


void SetIdentityMatrix(Matrix4x4 &m){
	for(int i = 0; i < 16; i++){
		if(i % 5 == 0) m[i] = 1; 
		else m[i] = 0; 
	}
}

void SetZeroMatrix(Matrix4x4 &m){
	for(int i = 0; i < 16; i++){
		m[i] = 0; 
	}
}

void SetZeroMatrix(Matrix3x3 &m){
	for(int i = 0; i < 9; i++){
		m[i] = 0; 
	}
}

void SetMatrix(Matrix4x4 &m1, Matrix4x4 m2){
	for(int i = 0; i < 16; i++){
		m1[i] = m2[i]; 
	}
}

void SetMatrix(Matrix3x3 &m1, Matrix3x3 m2){
	for(int i = 0; i < 9; i++){
		m1[i] = m2[i]; 
	}
}

void SetMatrixEntry(Matrix4x4 &m, float entry, int index){
		if(index >= 0 && index < 16){
				m[index] = entry; 
		}
}

void SetMatrixEntry(Matrix4x4 &m, float entry, int row, int col){
		if(row >= 0 && row < 4 && col >= 0 && col < 4){
				m[row * 4 + col] = entry; 
		}
}

float GetMatrixEntry(Matrix4x4 m, int row, int col)
{
		float entry = 0.0f; 
		if(row >= 0 && row < 4 && col >= 0 && col < 4){
				entry = m[row * 4 + col]; 
		}
		return entry; 
}

/**
 *  ( xx(1-c)+c	    xy(1-c)-zs  xz(1-c)+ys	0	)
 *  | yx(1-c)+zs	yy(1-c)+c   yz(1-c)-xs	0	|
 *  | xz(1-c)-ys	yz(1-c)+xs  zz(1-c)+c	0	|
 *  (	 0				0			0		1	)
 */
void SetRotationMatrix(Matrix4x4 &m, float angle, float x, float y, float z){
	SetIdentityMatrix(m); 
	float c = cos(angle * M_PI / 180); 
	float s = sin(angle * M_PI / 180); 
	m[0] = x * x * (1 - c) + c; 
	m[1] = y * x * (1 - c) + z * s; 
	m[2] = x * z * (1 - c) - y * s; 
	m[4] = x * y * (1 - c) - z * s; 
	m[5] = y * y * (1 - c) + c; 
	m[6] = y * z * (1 - c) + x * s; 
	m[8] = x * z * (1 - c) + y * s; 
	m[9] = y * z * (1 - c) - x * s; 
	m[10] = z * z * (1 - c) + c; 
}

void SetTranslationMatrix(Matrix4x4 &m, float transX, float transY, float transZ){
	SetIdentityMatrix(m); 
	m[12] = transX; 
	m[13] = transY; 
	m[14] = transZ; 
}

void SetScaleMatrix(Matrix4x4 &m, float scaleX, float scaleY, float scaleZ){
	SetIdentityMatrix(m); 
	m[0] = scaleX; 
	m[5] = scaleY; 
	m[10] = scaleZ; 
}

void SetPerspectiveMatrix(Matrix4x4 &output, float fovy, float aspect, float nearDist, float farDist){
	float f = CoTan(fovy / 2); 

	SetIdentityMatrix(output); 

	output[0] = f / aspect; 
	output[5] = f; 
	output[10] = (farDist + nearDist) / (nearDist - farDist); 
	output[11] = -1.0f; 
	output[14] = (2 * farDist * nearDist) / (nearDist - farDist); 
	output[15] = 0.0f; 
}

//void SetLookAtMatrix(Matrix4x4 &output, geometry::Vector3D<float> pos, geometry::Vector3D<float> at, geometry::Vector3D<float> up){
void SetLookAtMatrix(Matrix4x4 &output, float posX, float posY, float posZ, float atX, float atY, float atZ, float upX, float upY, float upZ){
	geometry::Vector3D<float> pos, at, up, f, upN, s, u, atN; 
	pos.SetValues(posX, posY, posZ); 
	at.SetValues(atX, atY, atZ); 
	up.SetValues(upX, upY, upZ); 
	f[0] = at[0] - pos[0]; 
	f[1] = at[1] - pos[1]; 
	f[2] = at[2] - pos[2]; 
	//NormalizeVector(f); 
	f.Normalize(); 
	
	//SetVector3(upN, up); 
	upN = up; 
	//NormalizeVector(upN); 
	upN.Normalize();

	//SetVector3(atN, at); 
	atN = at; 
	//NormalizeVector(atN); 
	atN.Normalize();

	s = f.GetCrossProduct(upN); 
	s.Normalize(); 
	u = s.GetCrossProduct(f); 
	u.Normalize(); 
/*	CalcCrossProduct(s, f, upN); 
	NormalizeVector(s); 
	CalcCrossProduct(u, s, f); 
	NormalizeVector(u);*/ 

	SetIdentityMatrix(output); 

	output[0] = s[0]; 
	output[4] = s[1]; 
	output[8] = s[2]; 

	output[1] = u[0]; 
	output[5] = u[1]; 
	output[9] = u[2]; 

	output[2] = -f[0]; 
	output[6] = -f[1]; 
	output[10] = -f[2]; 

	Matrix4x4 trans; 
	SetTranslationMatrix(trans, -pos[0], -pos[1], -pos[2]); 
	MultMatrix(output, trans); 
}

void MultMatrix(Matrix4x4 &m1, Matrix4x4 m2){
	Matrix4x4 m; 
	SetZeroMatrix(m); 
	for(int y = 0; y < 4; y++){
		for(int x = 0; x < 4; x++){
			for(int i = 0; i < 4; i++){
				m[y * 4 + x] += m1[i * 4 + x] * m2[y * 4 + i]; 
			}
		}
	}
	SetMatrix(m1, m); 
}

void PrintMatrix(Matrix4x4 m){
	printf("\n"); 
	for(int y = 0; y < 4; y++){
		printf("\n"); 
		for(int x = 0; x < 4; x++){
			printf("%f \t", m[y * 4 + x]); 
		}
	}
	printf("\n\n"); 
}

float* ConvertMatrixToArray(Matrix4x4 m){
	float* f = new float[16]; 
	for(int i = 0; i < 16; i++){
		f[i] = m[i]; 
	}
	return f; 
}

//float GetDeterminant(Matrix4x4 m){
//	return m[0] * m[4] * m[8] + m[1] * m[5] * m[6] + 
//		m[2] * m[3] * m[7] - m[0] * m[5] * m[7] - 
//		m[1] * m[3] * m[8] - m[2] * m[4] * m[6];
//}

void GetTransposeMatrix(Matrix4x4 &copyMatrix, Matrix4x4 m){
	SetMatrix(copyMatrix, m); 
	arsMat4x4GetTranspose(copyMatrix); 
}

void GetInverseMatrix(Matrix4x4 &copyMatrix, Matrix4x4 m){
	SetMatrix(copyMatrix, m); 

	arsMat4x4GetTranspose(copyMatrix); 
	arsMat4x4GetInverse(copyMatrix, copyMatrix); 
	arsMat4x4GetTranspose(copyMatrix); 
}

void GetInverseMatrix(Matrix3x3 &copyMatrix, Matrix4x4 m){
	SetMatrix(copyMatrix, m); 

	float det = 
		m[0] * m[4] * m[8] + m[1] * m[5] * m[6] + 
		m[2] * m[3] * m[7] - m[0] * m[5] * m[7] - 
		m[1] * m[3] * m[8] - m[2] * m[4] * m[6]; 

	copyMatrix[0] = (m[4] * m[8] - m[5] * m[7]) / det; 
	copyMatrix[1] = (m[2] * m[7] - m[1] * m[8]) / det; 
	copyMatrix[2] = (m[1] * m[5] - m[2] * m[4]) / det; 
	copyMatrix[3] = (m[5] * m[6] - m[3] * m[8]) / det; 
	copyMatrix[4] = (m[0] * m[8] - m[2] * m[6]) / det; 
	copyMatrix[5] = (m[2] * m[3] - m[0] * m[5]) / det; 
	copyMatrix[6] = (m[3] * m[7] - m[4] * m[6]) / det; 
	copyMatrix[7] = (m[1] * m[6] - m[0] * m[7]) / det; 
	copyMatrix[8] = (m[0] * m[4] - m[1] * m[3]) / det; 
}

//void SetHomographyMatrix(Matrix4x4 &output, float** sourcePoints, float** targetPoints){
void SetHomographyMatrix(Matrix4x4 &output, float sourcePoints[4][3], float targetPoints[4][3]){
	Matrix3x3 sourceMatrix, targetMatrix, inverseSourceMatrix; 
	SetUnitSquareHomographyMatrix(sourceMatrix, sourcePoints[0][0], sourcePoints[1][0], 
		sourcePoints[2][0], sourcePoints[3][0], 
		sourcePoints[0][1], sourcePoints[1][1], 
		sourcePoints[2][1], sourcePoints[3][1]); 
	SetUnitSquareHomographyMatrix(targetMatrix, targetPoints[0][0], targetPoints[1][0], 
		targetPoints[2][0], targetPoints[3][0], 
		targetPoints[0][1], targetPoints[1][1], 
		targetPoints[2][1], targetPoints[3][1]); 
	GetInverseMatrix(inverseSourceMatrix, sourceMatrix); 
	ConcatHomographyMatrizes(output, inverseSourceMatrix, targetMatrix); 
}

void SetUnitSquareHomographyMatrix(Matrix3x3 &output, float x1, float x2, float x3, float x4, float y1, float y2, float y3, float y4){
	SetZeroMatrix(output); 
	float S = (x2 - x3) * (y4 - y3) - (x4 - x3) * (y2 - y3); 

	float a31 = ((x1 - x2 + x3 - x4) * (y4 - y3) - (y1 - y2 + y3 - y4) * (x4 - x3)) / S; 
	float a32 = ((y1 - y2 + y3 - y4) * (x2 - x3) - (x1 - x2 + x3 - x4) * (y2 - y3)) / S; 

	float a11 = x2 - x1 + a31 * x2; 
	float a12 = x4 - x1 + a32 * x4; 
	float a13 = x1; 

	float a21 = y2 - y1 + a31 * y2; 
	float a22 = y4 - y1 + a32 * y4; 
	float a23 = y1; 

	output[0] = a11; 
	output[1] = a12; 
	output[2] = a13; 
	output[3] = a21; 
	output[4] = a22; 
	output[5] = a23; 
	output[6] = a31; 
	output[7] = a32; 
	output[8] = 1.0f; 
}

void ConcatHomographyMatrizes(Matrix4x4 &output, Matrix3x3 sourceMatrix, Matrix3x3 targetMatrix){
	output[0] = targetMatrix[0] * sourceMatrix[0] + targetMatrix[1] * sourceMatrix[3] + targetMatrix[2] * sourceMatrix[6]; 
	output[4] = targetMatrix[0] * sourceMatrix[1] + targetMatrix[1] * sourceMatrix[4] + targetMatrix[2] * sourceMatrix[7]; 
	output[12] = targetMatrix[0] * sourceMatrix[2] + targetMatrix[1] * sourceMatrix[5] + targetMatrix[2] * sourceMatrix[8]; 

	output[1] = targetMatrix[3] * sourceMatrix[0] + targetMatrix[4] * sourceMatrix[3] + targetMatrix[5] * sourceMatrix[6]; 
	output[5] = targetMatrix[3] * sourceMatrix[1] + targetMatrix[4] * sourceMatrix[4] + targetMatrix[5] * sourceMatrix[7]; 
	output[13] = targetMatrix[3] * sourceMatrix[2] + targetMatrix[4] * sourceMatrix[5] + targetMatrix[5] * sourceMatrix[8]; 

	output[3] = targetMatrix[6] * sourceMatrix[0] + targetMatrix[7] * sourceMatrix[3] + targetMatrix[8] * sourceMatrix[6]; 
	output[7] = targetMatrix[6] * sourceMatrix[1] + targetMatrix[7] * sourceMatrix[4] + targetMatrix[8] * sourceMatrix[7]; 
	output[15] = targetMatrix[6] * sourceMatrix[2] + targetMatrix[7] * sourceMatrix[5] + targetMatrix[8] * sourceMatrix[8]; 

	//ignore z-axis
	output[2] = 0.0; 
	output[6] = 0.0; 
	output[8] = 0.0; 
	output[9] = 0.0; 
	output[10] = 1.0; 
	output[11] = 0.0; 
	output[14] = 0.0; 
}


