/***************************************************************************
 * matrix and vector operations                                            *
 * Note that the provided functions are provided for 4x4 matrices and 4x1  *
 * vectors (as OpenGL uses them). The 4x4 matrix is saved in an array of   *
 * 16 values. The first row is saved in the first 4 positions (0 to 3), at *
 * position 4 the first column of the second row is written, at position 5 *
 * the second column of the second row, etc.                               *
 *                                                                         *
 * authors: Ines Stuppacher, Peter Supan                                   *
 * last update: 2005/06/01                                                 *
 **************************************************************************/

#include <math.h>
#include <stdlib.h>
//#include <stdio.h>
#include <cstdio>

#define _USE_MATH_DEFINES
#include <math.h>

#include "arsMath.h"

static float *minv( float *ap, int dimen, int rowa );


//--- functions for 4x4 matrices ---------------------------------------
void arsMat4x4SetValues(ARSMatrix4x4 matrix,
				 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 ){

	matrix[ 0]=a; matrix[ 1]=b; matrix[ 2]=c; matrix[3]=d;	
	matrix[ 4]=e; matrix[ 5]=f; matrix[ 6]=g; matrix[7]=h;
	matrix[ 8]=i; matrix[ 9]=j; matrix[10]=k; matrix[11]=l;
	matrix[12]=m; matrix[13]=n; matrix[14]=o; matrix[15]=p;
}

void arsMat4x4MatrixSetIdentity(ARSMatrix4x4 matrix){
	arsMat4x4SetValues(matrix, 
					1, 0, 0, 0, 
					0, 1, 0, 0, 
					0, 0, 1, 0, 
					0, 0, 0, 1);
}

void arsMat4x4SetValuesOfMatrix(ARSMatrix4x4 source, ARSMatrix4x4 destination){
    int i;
    
    for(i = 0; i < 16; i++) {
        destination[i] = source[i];
    }
}

void arsMat4x4GetTranspose(ARSMatrix4x4 matrix){
	arsMat4x4SetValues(matrix,
					matrix[ 0], matrix[ 4], matrix[ 8], matrix[12],
					matrix[ 1], matrix[ 5], matrix[ 9], matrix[13],
					matrix[ 2], matrix[ 6], matrix[10], matrix[14],
					matrix[ 3], matrix[ 7], matrix[11], matrix[15]);
}

void arsMat4x4GetInverse(ARSMatrix4x4 source, ARSMatrix4x4 destination){
	float tmp[12]; /* temp array for pairs */
	float src[16]; /* array of transpose source matrix */
	float det; /* determinant */
	
	/* transpose matrix */
	for ( int i = 0; i < 4; i++) {
		src[i] = source[i*4];
		src[i + 4] = source[i*4 + 1];
		src[i + 8] = source[i*4 + 2];
		src[i + 12] = source[i*4 + 3];
	}

	/* calculate pairs for first 8 elements (cofactors) */
	tmp[0] = src[10] * src[15];
	tmp[1] = src[11] * src[14];
	tmp[2] = src[9] * src[15];
	tmp[3] = src[11] * src[13];
	tmp[4] = src[9] * src[14];
	tmp[5] = src[10] * src[13];
	tmp[6] = src[8] * src[15];
	tmp[7] = src[11] * src[12];
	tmp[8] = src[8] * src[14];
	tmp[9] = src[10] * src[12];
	tmp[10] = src[8] * src[13];
	tmp[11] = src[9] * src[12];

	/* calculate first 8 elements (cofactors) */
	destination[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
	destination[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
	destination[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
	destination[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
	destination[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
	destination[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
	destination[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
	destination[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
	destination[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
	destination[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
	destination[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
	destination[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
	destination[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
	destination[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
	destination[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
	destination[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];

	/* calculate pairs for second 8 elements (cofactors) */
	tmp[0] = src[2]*src[7];
	tmp[1] = src[3]*src[6];
	tmp[2] = src[1]*src[7];
	tmp[3] = src[3]*src[5];
	tmp[4] = src[1]*src[6];
	tmp[5] = src[2]*src[5];

	tmp[6] = src[0]*src[7];
	tmp[7] = src[3]*src[4];
	tmp[8] = src[0]*src[6];
	tmp[9] = src[2]*src[4];
	tmp[10] = src[0]*src[5];
	tmp[11] = src[1]*src[4];

	/* calculate second 8 elements (cofactors) */
	destination[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
	destination[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
	destination[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
	destination[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
	destination[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
	destination[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
	destination[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
	destination[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
	destination[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
	destination[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
	destination[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
	destination[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
	destination[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
	destination[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
	destination[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
	destination[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];

	/* calculate determinant */
	det=src[0]*destination[0]+src[1]*destination[1]+src[2]*destination[2]+src[3]*destination[3];
	
	/* calculate matrix inverse */
	det = 1/det;
	for ( int j = 0; j < 16; j++)
		destination[j] *= det;
}

void arsMat4x4Mat4x4Mult(ARSMatrix4x4 mat1, ARSMatrix4x4 mat2, ARSMatrix4x4 matOut){
	matOut[0] = mat1[0] * mat2[0] + mat1[1] * mat2[4] + mat1[2] * mat2[ 8] + mat1[3] * mat2[12];
	matOut[1] = mat1[0] * mat2[1] + mat1[1] * mat2[5] + mat1[2] * mat2[ 9] + mat1[3] * mat2[13];
	matOut[2] = mat1[0] * mat2[2] + mat1[1] * mat2[6] + mat1[2] * mat2[10] + mat1[3] * mat2[14];
	matOut[3] = mat1[0] * mat2[3] + mat1[1] * mat2[7] + mat1[2] * mat2[11] + mat1[3] * mat2[15];

	matOut[4] = mat1[4] * mat2[0] + mat1[5] * mat2[4] + mat1[6] * mat2[ 8] + mat1[7] * mat2[12];
	matOut[5] = mat1[4] * mat2[1] + mat1[5] * mat2[5] + mat1[6] * mat2[ 9] + mat1[7] * mat2[13];
	matOut[6] = mat1[4] * mat2[2] + mat1[5] * mat2[6] + mat1[6] * mat2[10] + mat1[7] * mat2[14];
	matOut[7] = mat1[4] * mat2[3] + mat1[5] * mat2[7] + mat1[6] * mat2[11] + mat1[7] * mat2[15];

	matOut[ 8] = mat1[8] * mat2[0] + mat1[9] * mat2[4] + mat1[10] * mat2[ 8] + mat1[11] * mat2[12];
	matOut[ 9] = mat1[8] * mat2[1] + mat1[9] * mat2[5] + mat1[10] * mat2[ 9] + mat1[11] * mat2[13];
	matOut[10] = mat1[8] * mat2[2] + mat1[9] * mat2[6] + mat1[10] * mat2[10] + mat1[11] * mat2[14];
	matOut[11] = mat1[8] * mat2[3] + mat1[9] * mat2[7] + mat1[10] * mat2[11] + mat1[11] * mat2[15];

	matOut[12] = mat1[12] * mat2[0] + mat1[13] * mat2[4] + mat1[14] * mat2[ 8] + mat1[15] * mat2[12];
	matOut[13] = mat1[12] * mat2[1] + mat1[13] * mat2[5] + mat1[14] * mat2[ 9] + mat1[15] * mat2[13];
	matOut[14] = mat1[12] * mat2[2] + mat1[13] * mat2[6] + mat1[14] * mat2[10] + mat1[15] * mat2[14];
	matOut[15] = mat1[12] * mat2[3] + mat1[13] * mat2[7] + mat1[14] * mat2[11] + mat1[15] * mat2[15];
}


void arsVec4Mat4x4Mult(ARSVector4 vecIn, ARSMatrix4x4 matIn, ARSVector4 vecOut){
	vecOut[0] = (vecIn[0]*matIn[ 0]) + (vecIn[1]*matIn[ 1]) + (vecIn[2]*matIn[ 2]) + (vecIn[3]*matIn[ 3]);
	vecOut[1] = (vecIn[0]*matIn[ 4]) + (vecIn[1]*matIn[ 5]) + (vecIn[2]*matIn[ 6]) + (vecIn[3]*matIn[ 7]);
	vecOut[2] = (vecIn[0]*matIn[ 8]) + (vecIn[1]*matIn[ 9]) + (vecIn[2]*matIn[10]) + (vecIn[3]*matIn[11]);
	vecOut[3] = (vecIn[0]*matIn[12]) + (vecIn[1]*matIn[13]) + (vecIn[2]*matIn[14]) + (vecIn[3]*matIn[15]);
  
	vecOut[0] /= vecOut[3];
	vecOut[1] /= vecOut[3];
	vecOut[2] /= vecOut[3];
	vecOut[3] = 1.0;
}

float arsVec4Length(const ARSVector4 vector){
    return sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
}

float arsVec4LengthSq(const ARSVector4 vector){
    return vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2];
}

float arsVec4Dot(ARSVector4 vecA, ARSVector4 vecB){
    return vecA[0]*vecB[0] + vecA[1]*vecB[1] + vecA[2]*vecB[2] + vecA[3]*vecB[3];
}

void arsVec4Cross(ARSVector4 vecA, ARSVector4 vecB, ARSVector4 vecOut){
    vecOut[0] = vecA[1]*vecB[2] - vecA[2]*vecB[1];
    vecOut[1] = -1.0 * (vecA[0]*vecB[2] - vecA[2]*vecB[0]);
    vecOut[2] = vecA[0]*vecB[1] - vecA[1]*vecB[0];
    vecOut[3] = 1.0;
}

void arsVec4Add(ARSVector4 vecA, ARSVector4 vecB, ARSVector4 vecOut){
    vecOut[0] = vecA[0] + vecB[0];
    vecOut[1] = vecA[1] + vecB[1];
    vecOut[2] = vecA[2] + vecB[2];
    vecOut[3] = 1.0;
}

void arsVec4Subtract(ARSVector4 vecA, ARSVector4 vecB, ARSVector4 vecOut){
    vecOut[0] = vecA[0] - vecB[0];
    vecOut[1] = vecA[1] - vecB[1];
    vecOut[2] = vecA[2] - vecB[2];
    vecOut[3] = 1.0;
}

void arsVec4Normalize(ARSVector4 vector){
    float length = arsVec4Length(vector);
    vector[0] /= length;
    vector[1] /= length;
    vector[2] /= length;
}


//--- functions for transforming 4x4 matrices ----------------------------
void arsTranslatef(ARSMatrix4x4 matrix, float x, float y, float z){
    ARSMatrix4x4 translation;
    ARSMatrix4x4 result;

    arsMat4x4MatrixSetIdentity(translation);
    translation[ 3] = x;
    translation[ 7] = y;
    translation[11] = z;

    arsMat4x4Mat4x4Mult(matrix, translation, result);
    arsMat4x4SetValuesOfMatrix(result, matrix);
}

void arsRotatef(ARSMatrix4x4 matrix, float angle, float x, float y, float z){
    ARSMatrix4x4 rotation;
    ARSMatrix4x4 result;
    ARSVector4 vec = {x, y, z, 1.0};

    float cosinus = cos(angle/180.0*M_PI);
    float sinus = sin(angle/180.0*M_PI);
    if (arsVec4LengthSq(vec) != 1.0) {
        arsVec4Normalize(vec);
    }

    arsMat4x4MatrixSetIdentity(rotation);
    rotation[ 0] = vec[0] * vec[0] * (1 - cosinus) + cosinus;
    rotation[ 1] = vec[0] * vec[1] * (1 - cosinus) - vec[2] * sinus;
    rotation[ 2] = vec[0] * vec[2] * (1 - cosinus) + vec[1] * sinus;

    rotation[ 4] = vec[1] * vec[0] * (1 - cosinus) + vec[2] * sinus;
    rotation[ 5] = vec[1] * vec[1] * (1 - cosinus) + cosinus;
    rotation[ 6] = vec[1] * vec[2] * (1 - cosinus) - vec[0] * sinus;

    rotation[ 8] = vec[0] * vec[2] * (1 - cosinus) - vec[1] * sinus;
    rotation[ 9] = vec[1] * vec[2] * (1 - cosinus) + vec[0] * sinus;
    rotation[10] = vec[2] * vec[2] * (1 - cosinus) + cosinus;

    arsMat4x4Mat4x4Mult(matrix, rotation, result);
    arsMat4x4SetValuesOfMatrix(result, matrix);
}

void arsScalef(ARSMatrix4x4 matrix, float x, float y, float z){
    ARSMatrix4x4 scale;
    ARSMatrix4x4 result;

    arsMat4x4MatrixSetIdentity(scale);
    scale[ 0] = x;
    scale[ 5] = y;
    scale[10] = z;

    arsMat4x4Mat4x4Mult(matrix, scale, result);
    arsMat4x4SetValuesOfMatrix(result, matrix);
}

void arsPerspective(ARSMatrix4x4 matrix, float fovy, float aspect, float zNear, float zFar) {
    ARSMatrix4x4 perspective;
    ARSMatrix4x4 result;

    arsMat4x4MatrixSetIdentity(perspective);

    float focusLength = 1.0/(tan ((fovy/180.0 * M_PI)/2.0));
    perspective[ 0] = focusLength/aspect;
    perspective[ 5] = focusLength;
    perspective[10] = (zFar + zNear)/(zNear - zFar);
    perspective[11] = (2.0*zFar*zNear)/(zNear - zFar);
    perspective[14] = -1.0;
    perspective[15] =  0.0;

    arsMat4x4Mat4x4Mult(matrix, perspective, result);
    arsMat4x4SetValuesOfMatrix(result, matrix);
}

void arsFrustum(ARSMatrix4x4 matrix, float left, float right, float bottom, float top, float zNear, float zFar) {

    float x, y, a, b, c, d;

    x = (2.0F*zNear) / (right-left);
    y = (2.0F*zNear) / (top-bottom);
    a = (right+left) / (right-left);
    b = (top+bottom) / (top-bottom);
    c = -(zFar+zNear) / ( zFar-zNear);
    d = -(2.0F*zFar*zNear) / (zFar-zNear);  /* error? */

#define M(row,col)  matrix[col*4+row]
    M(0,0) = x;     M(0,1) = 0.0F;  M(0,2) = a;      M(0,3) = 0.0F;
    M(1,0) = 0.0F;  M(1,1) = y;     M(1,2) = b;      M(1,3) = 0.0F;
    M(2,0) = 0.0F;  M(2,1) = 0.0F;  M(2,2) = c;      M(2,3) = d;
    M(3,0) = 0.0F;  M(3,1) = 0.0F;  M(3,2) = -1.0F;  M(3,3) = 0.0F;
#undef M
}


//--- functions for debugging --------------------------------------------
void arsMat4x4Print(ARSMatrix4x4 matrix){
    int i;

    for(i = 0; i < 16; i++) {
        printf("%f ", matrix[i]);
        if((i+1)%4 == 0) {
            printf("\n");
        }
    }
}

void arsVec4Print(ARSVector4 vector){
    printf("%f / %f / %f / %f\n", vector[0], vector[1], vector[2], vector[3]);
}


//--- vector3 (used to reconstruct D3DXVECTOR3 used in NVMeshMender) ---
ARSVector3::ARSVector3(){
	this->x = this->y = this->z = 0.0;
}

ARSVector3::ARSVector3(const float* values){
	this->x = values[0];
	this->y = values[1];
	this->z = values[2];
}

ARSVector3::ARSVector3(const ARSVector3 &vector){
	this->x = vector.x;
	this->y = vector.y;
	this->z = vector.z;
}

ARSVector3::ARSVector3(float x, float y, float z){
	this->x = x;
	this->y = y;
	this->z = z;
}

//--- overload operators -----------------------------------------------
ARSVector3::operator float* (){
	float vector[3];

	vector[0] = this->x;
	vector[1] = this->y;
	vector[2] = this->z;

	return vector;
}

ARSVector3::operator const float* () const{
	float vector[3];

	vector[0] = this->x;
	vector[1] = this->y;
	vector[2] = this->z;

	return vector;
}

ARSVector3& ARSVector3::operator = (const ARSVector3 &vector){
	this->x = vector.x;
	this->y = vector.y;
	this->z = vector.z;

	return *this;
}

ARSVector3& ARSVector3::operator += (const ARSVector3 &vector){
	this->x += vector.x;
	this->y += vector.y;
	this->z += vector.z;

	return *this;
}

ARSVector3& ARSVector3::operator -= (const ARSVector3 &vector){
	this->x -= vector.x;
	this->y -= vector.y;
	this->z -= vector.z;

	return *this;
}

ARSVector3& ARSVector3::operator *= (float value){
	this->x *= value;
	this->y *= value;
	this->z *= value;

	return *this;
}

ARSVector3& ARSVector3::operator /= (float value){
	this->x /= value;
	this->y /= value;
	this->z /= value;

	return *this;
}

ARSVector3 ARSVector3::operator + () const{
	ARSVector3 result;

	result.x = this->x;
	result.y = this->y;
	result.z = this->z;

	return result;
}

ARSVector3 ARSVector3::operator - () const{
	ARSVector3 result;

	result.x = this->x * -1.0;
	result.y = this->y * -1.0;
	result.z = this->z * -1.0;

	return result;
}

ARSVector3 ARSVector3::operator + (const ARSVector3 &vector) const{
	ARSVector3 result;

	result.x = this->x + vector.x;
	result.y = this->y + vector.y;
	result.z = this->z + vector.z;

	return result;
}

ARSVector3 ARSVector3::operator - (const ARSVector3 &vector) const{
	ARSVector3 result;

	result.x = this->x - vector.x;
	result.y = this->y - vector.y;
	result.z = this->z - vector.z;

	return result;
}

ARSVector3 ARSVector3::operator * (const float value) const{
	ARSVector3 result;

	result.x = this->x * value;
	result.y = this->y * value;
	result.z = this->z * value;

	return result;
}

ARSVector3 ARSVector3::operator / (const float value) const{
	ARSVector3 result;

	result.x = this->x / value;
	result.y = this->y / value;
	result.z = this->z / value;

	return result;
}

ARSVector3 operator * (float value, const struct ARSVector3 &vector){  // friend
	ARSVector3 result;

	result.x = vector.x * value;
	result.y = vector.y * value;
	result.z = vector.z * value;

	return result;
}

bool ARSVector3::operator == (const ARSVector3 &vector) const{
	if(this->x == vector.x && this->y == vector.y && this->z == vector.z) {
		return true;
	} else {
		return false;
	}
}

bool ARSVector3::operator != (const ARSVector3 &vector) const{
	if(this->x == vector.x && this->y == vector.y && this->z == vector.z) {
		return false;
	} else {
		return true;
	}
}

// set-methods
void ARSVector3::arsSetX(float x){
	this->x = x;
}

void ARSVector3::arsSetY(float y){
	this->y = y;
}

void ARSVector3::arsSetZ(float z){
	this->z = z;
}

// get-methods
float ARSVector3::arsGetX(void) const{
	return this->x;
}

float ARSVector3::arsGetY(void) const{
	return this->y;
}

float ARSVector3::arsGetZ(void) const{
	return this->z;
}

//--- functions for ARSVector3 -----------------------------------------
float arsVector3Length(const ARSVector3* vector){
	return sqrt(pow(vector->x, 2) + pow(vector->y, 2) + pow(vector->z, 2));
}

float arsVector3LengthSq(const ARSVector3* vector){
	return pow(vector->x, 2) + pow(vector->y, 2) + pow(vector->z, 2);
}

float arsVector3Dot(const ARSVector3* vectorA, const ARSVector3* vectorB){
	return vectorA->x*vectorB->x + vectorA->y*vectorB->y + vectorA->z*vectorB->z;
}

ARSVector3* arsVector3Cross(ARSVector3* out, const ARSVector3* vectorA, const ARSVector3* vectorB){
	out->arsSetX(vectorA->y*vectorB->z - vectorA->z*vectorB->y);
	out->arsSetY(-1.0 * (vectorA->x*vectorB->z - vectorA->z*vectorB->x));
	out->arsSetZ(vectorA->x*vectorB->y - vectorA->y*vectorB->x);

	return out;
}

ARSVector3* arsVector3Add(ARSVector3* out, const ARSVector3* vectorA, const ARSVector3* vectorB){
	*out = *vectorA + *vectorB;
	return out;
}

ARSVector3* arsVector3Subtract(ARSVector3* out, const ARSVector3* vectorA, const ARSVector3* vectorB){
	*out = *vectorA - *vectorB;
	return out;
}

ARSVector3* arsVector3Normalize(ARSVector3* out, const ARSVector3* in){
	*out = *in / arsVector3Length(in);
	return out;
}
