#include <lib/InsMatrix.h>

#include <math.h>

#define PI_OVER_180          0.01745329251994329444f
#define EPSILON_E3          (float)(1E-3)
#define CompareFloat(a, b)  ((fabs(a-b)<EPSILON_E3)?1:0)   //比较两个浮点数是否相等，相等为1，不等为0

GLfloat *MultMatrix4x4(GLfloat *matC, GLfloat *matA, GLfloat *matB)
{
    GLfloat matT[4*4];
	matT[ 0] = matA[ 0] * matB[ 0] + matA[ 1] * matB[ 4] + matA[ 2] * matB[ 8] + matA[ 3] * matB[12];
	matT[ 1] = matA[ 0] * matB[ 1] + matA[ 1] * matB[ 5] + matA[ 2] * matB[ 9] + matA[ 3] * matB[13];
	matT[ 2] = matA[ 0] * matB[ 2] + matA[ 1] * matB[ 6] + matA[ 2] * matB[10] + matA[ 3] * matB[14];
	matT[ 3] = matA[ 0] * matB[ 3] + matA[ 1] * matB[ 7] + matA[ 2] * matB[11] + matA[ 3] * matB[15];
	matT[ 4] = matA[ 4] * matB[ 0] + matA[ 5] * matB[ 4] + matA[ 6] * matB[ 8] + matA[ 7] * matB[12];
	matT[ 5] = matA[ 4] * matB[ 1] + matA[ 5] * matB[ 5] + matA[ 6] * matB[ 9] + matA[ 7] * matB[13];
	matT[ 6] = matA[ 4] * matB[ 2] + matA[ 5] * matB[ 6] + matA[ 6] * matB[10] + matA[ 7] * matB[14];
	matT[ 7] = matA[ 4] * matB[ 3] + matA[ 5] * matB[ 7] + matA[ 6] * matB[11] + matA[ 7] * matB[15];
	matT[ 8] = matA[ 8] * matB[ 0] + matA[ 9] * matB[ 4] + matA[10] * matB[ 8] + matA[11] * matB[12];
	matT[ 9] = matA[ 8] * matB[ 1] + matA[ 9] * matB[ 5] + matA[10] * matB[ 9] + matA[11] * matB[13];
	matT[10] = matA[ 8] * matB[ 2] + matA[ 9] * matB[ 6] + matA[10] * matB[10] + matA[11] * matB[14];
	matT[11] = matA[ 8] * matB[ 3] + matA[ 9] * matB[ 7] + matA[10] * matB[11] + matA[11] * matB[15];
	matT[12] = matA[12] * matB[ 0] + matA[13] * matB[ 4] + matA[14] * matB[ 8] + matA[15] * matB[12];
	matT[13] = matA[12] * matB[ 1] + matA[13] * matB[ 5] + matA[14] * matB[ 9] + matA[15] * matB[13];
	matT[14] = matA[12] * matB[ 2] + matA[13] * matB[ 6] + matA[14] * matB[10] + matA[15] * matB[14];
	matT[15] = matA[12] * matB[ 3] + matA[13] * matB[ 7] + matA[14] * matB[11] + matA[15] * matB[15];

    matC[ 0] = matT[ 0]; matC[ 1] = matT[ 1]; matC[ 2] = matT[ 2]; matC[ 3] = matT[ 3];
    matC[ 4] = matT[ 4]; matC[ 5] = matT[ 5]; matC[ 6] = matT[ 6]; matC[ 7] = matT[ 7];
    matC[ 8] = matT[ 8]; matC[ 9] = matT[ 9]; matC[10] = matT[10]; matC[11] = matT[11];
    matC[12] = matT[12]; matC[13] = matT[13]; matC[14] = matT[14]; matC[15] = matT[15];

    return matC;
}

GLfloat *MultMatrix4x4Vec4x1(GLfloat *vecB, GLfloat *matA, GLfloat *vecA)
{
	GLfloat vecTmp[4];

    vecTmp[0] = vecA[0]*matA[0*4+0] + vecA[1]*matA[1*4+0] + vecA[2]*matA[2*4+0] + vecA[3]*matA[3*4+0];
    vecTmp[1] = vecA[0]*matA[0*4+1] + vecA[1]*matA[1*4+1] + vecA[2]*matA[2*4+1] + vecA[3]*matA[3*4+1];
    vecTmp[2] = vecA[0]*matA[0*4+2] + vecA[1]*matA[1*4+2] + vecA[2]*matA[2*4+2] + vecA[3]*matA[3*4+2];
    vecTmp[3] = vecA[0]*matA[0*4+3] + vecA[1]*matA[1*4+3] + vecA[2]*matA[2*4+3] + vecA[3]*matA[3*4+3];

    vecB[0] = vecTmp[0];
    vecB[1] = vecTmp[1];
    vecB[2] = vecTmp[2];
    vecB[3] = vecTmp[3];

    return vecB;
}

GLfloat *CopyMatrix4x4(GLfloat *matDes, GLfloat *matSrc)
{
    matDes[0] = matSrc[0]; matDes[1] = matSrc[1];
    matDes[2] = matSrc[2]; matDes[3] = matSrc[3];
    matDes[4] = matSrc[4]; matDes[5] = matSrc[5];
    matDes[6] = matSrc[6]; matDes[7] = matSrc[7];
    matDes[8] = matSrc[8]; matDes[9] = matSrc[9];
    matDes[10] = matSrc[10]; matDes[11] = matSrc[11];
    matDes[12] = matSrc[12]; matDes[13] = matSrc[13];
    matDes[14] = matSrc[14]; matDes[15] = matSrc[15];

    return matDes;
}

Bool InvertMatrix4x4(GLfloat *inverse, GLfloat *src)
{
	int i, j, k;
	double t;
	double temp[4][4];

	for (i=0; i<4; i++) {
		for (j=0; j<4; j++) {
			temp[i][j] = src[i*4+j];
		}
	}

    LoadIdentityMatrix(inverse);

	for (i = 0; i < 4; i++) {
		/*
		** Look for largest element in column
		*/
		int swap = i;
		for (j = i + 1; j < 4; j++) {
			if (fabs(temp[j][i]) > fabs(temp[i][i])) {
				swap = j;
			}
		}

		if (swap != i) {
			/*
			** Swap rows.
			*/
			for (k = 0; k < 4; k++) {
				t = temp[i][k];
				temp[i][k] = temp[swap][k];
				temp[swap][k] = t;

				t = inverse[i*4+k];
				inverse[i*4+k] = inverse[swap*4+k];
				inverse[swap*4+k] = t;
			}
		}

		if (temp[i][i] == 0) {
			/*
			** No non-zero pivot.  The matrix is singular, which shouldn't
			** happen.  This means the user gave us a bad matrix.
			*/
			return INS_FALSE;
		}

		t = temp[i][i];
		for (k = 0; k < 4; k++) {
			temp[i][k] /= t;
			inverse[i*4+k] /= t;
		}
		for (j = 0; j < 4; j++) {
			if (j != i) {
				t = temp[j][i];
				for (k = 0; k < 4; k++) {
					temp[j][k] -= temp[i][k]*t;
					inverse[j*4+k] -= inverse[i*4+k]*t;
				}
			}
		}
	}
	return INS_TRUE;
}

GLfloat *SetRotateMatrix4x4_X_Axis(GLfloat *m, GLfloat angle)
{
	float radians = PI_OVER_180*angle;

	LoadIdentityMatrix(m);

	m[5] = cosf(radians);
	m[6] = sinf(radians);
	m[9] = -m[6];//-sinf(radians);
	m[10] = m[5];//cosf(radians);
    return m;
}

GLfloat *RotateMatrix4x4_X_Axis(GLfloat *m, GLfloat angle)
{
    GLfloat tm[4*4];

    SetRotateMatrix4x4_X_Axis(tm, angle);
    MultMatrix4x4(m, tm, m);

    return m;
}

GLfloat *SetRotateMatrix4x4_Y_Axis(GLfloat *m, GLfloat angle)
{
	float radians = PI_OVER_180*angle;

	LoadIdentityMatrix(m);

	m[0] = cosf(radians);
	m[2] = -sinf(radians);
	m[8] = -m[2];//sinf(radians);
	m[10] = m[0];//cosf(radians);
    return m;
}

GLfloat *RotateMatrix4x4_Y_Axis(GLfloat *m, GLfloat angle)
{
    GLfloat tm[4*4];

    SetRotateMatrix4x4_Y_Axis(tm, angle);
    MultMatrix4x4(m, tm, m);

    return m;
}

GLfloat *SetRotateMatrix4x4_Z_Axis(GLfloat *m, GLfloat angle)
{
	float radians = PI_OVER_180*angle;

	LoadIdentityMatrix(m);

	m[0] = cosf(radians);
	m[1] = sinf(radians);
	m[4] = -m[1];//-sinf(radians);
	m[5] = m[0];//cosf(radians);
    return m;
}

GLfloat *RotateMatrix4x4_Z_Axis(GLfloat *m, GLfloat angle)
{
    GLfloat tm[4*4];

    SetRotateMatrix4x4_Z_Axis(tm, angle);
    MultMatrix4x4(m, tm, m);

    return m;
}

/*
* 绕任意轴向量axis_nor逆时针旋转angle度
*/
GLfloat *SetRotateMat4x4(GLfloat *mat4x4, Vec3f *axis_nor, GLfloat angle)
{
	float c, s, one_c;

	float xx, yy, zz;
	float xs, ys, zs;
	float xy, xz, yz;

    c = cosf(angle*PI_OVER_180);
    s = sinf(angle*PI_OVER_180);
    one_c = 1.0 - c;

	xx = axis_nor->x * axis_nor->x;
	yy = axis_nor->y * axis_nor->y;
	zz = axis_nor->z * axis_nor->z;

	xs = axis_nor->x * s;
	ys = axis_nor->y * s;
	zs = axis_nor->z * s;

	xy = axis_nor->x * axis_nor->y;
	xz = axis_nor->x * axis_nor->z;
	yz = axis_nor->y * axis_nor->z;

	mat4x4[ 0] = (one_c * xx) + c;
	mat4x4[ 1] = (one_c * xy) + zs;
	mat4x4[ 2] = (one_c * xz) - ys;
	mat4x4[ 3] = 0.0;

	mat4x4[ 4] = (one_c * xy) - zs;
	mat4x4[ 5] = (one_c * yy) + c;
	mat4x4[ 6] = (one_c * yz) + xs;
	mat4x4[ 7] = 0.0;

	mat4x4[ 8] = (one_c * xz) + ys;
	mat4x4[ 9] = (one_c * yz) - xs;
	mat4x4[10] = (one_c * zz) + c;
	mat4x4[11] = 0.0;

	mat4x4[12] = 0.0;
	mat4x4[13] = 0.0;
	mat4x4[14] = 0.0;
	mat4x4[15] = 1.0;

    return mat4x4;
}

/*
* 绕任意轴向量axis_nor逆时针旋转angle度
*/
GLfloat *RotateMat4x4(GLfloat *mat4x4, Vec3f *axis_nor, GLfloat angle)
{
    GLfloat tm[4*4];

    SetRotateMat4x4(tm, axis_nor, angle);
    MultMatrix4x4(mat4x4, tm, mat4x4);

    return mat4x4;
}

GLfloat *SetTranslateMatrix4x4 (GLfloat *m, GLfloat trX, GLfloat trY, GLfloat trZ)
{
	LoadIdentityMatrix(m);

	m[12] = trX;
	m[13] = trY;
	m[14] = trZ;

    return m;
}

GLfloat *TranslateMatrix4x4 (GLfloat *m, GLfloat trX, GLfloat trY, GLfloat trZ)
{
    GLfloat tm[4*4];

    SetTranslateMatrix4x4(tm, trX, trY, trZ);
    MultMatrix4x4(m, tm, m);

    return m;
}

GLfloat *SetScaleMatrix4x4 (GLfloat *m, GLfloat scaleX, GLfloat scaleY, GLfloat scaleZ)
{
	LoadIdentityMatrix(m);

	m[ 0] = scaleX;
	m[ 5] = scaleY;
	m[10] = scaleZ;

    return m;
}

GLfloat *ScaleMatrix4x4 (GLfloat *m, GLfloat scaleX, GLfloat scaleY, GLfloat scaleZ)
{
    GLfloat tm[4*4];

    SetScaleMatrix4x4(tm, scaleX, scaleY, scaleZ);
    MultMatrix4x4(m, tm, m);

    return m;
}