#include "StgSharpGraphic.h"
#include <stdio.h>
#include <pmmintrin.h>


SSGC_API matrix4map* add_mat4(matrix4map* mat1, matrix4map* mat2)
{
    matrix4map* ret = _aligned_malloc(sizeof(matrix4), 16);

	if (ret != 0)
	{
		ret->colum[0] = _mm_add_ps(mat1->colum[0], mat2->colum[0]);
		ret->colum[1] = _mm_add_ps(mat1->colum[1], mat2->colum[1]);
		ret->colum[2] = _mm_add_ps(mat1->colum[2], mat2->colum[2]);
		ret->colum[3] = _mm_add_ps(mat1->colum[3], mat2->colum[3]);
	}

	return ret;
}

SSGC_API matrix4map* sub_mat4(matrix4map* mat1, matrix4map* mat2)
{
	matrix4map* ret = _aligned_malloc(sizeof(matrix4), 16);

	if (ret != 0)
	{
		ret->colum[0] = _mm_sub_ps(mat1->colum[0], mat2->colum[0]);
		ret->colum[1] = _mm_sub_ps(mat1->colum[1], mat2->colum[1]);
		ret->colum[2] = _mm_sub_ps(mat1->colum[2], mat2->colum[2]);
		ret->colum[3] = _mm_sub_ps(mat1->colum[3], mat2->colum[3]);
	}

	return ret;
}

SSGC_API __inline matrix4map* __cdecl mul_const(matrix4map* mat, float value)
{
	matrix4map* ret = _aligned_malloc(sizeof(matrix4),16);

	if (NULL != ret)
	{
		float cstarray[4] = { value, value, value, value };

		__m128 cst128 = _mm_loadu_ps(&cstarray);

		ret->colum[0] = _mm_mul_ps(mat->colum[0], cst128);
		ret->colum[1] = _mm_mul_ps(mat->colum[1], cst128);
		ret->colum[2] = _mm_mul_ps(mat->colum[2], cst128);
		ret->colum[3] = _mm_mul_ps(mat->colum[3], cst128);
	}

	return ret;

}

//left is the transposed matrix of real left
SSGC_API __inline matrix4* __cdecl mul_4_4(matrix4map* left, matrix4map* right)
{
	matrix4map* retPtr = _aligned_malloc(sizeof(matrix4map), 16);
	if (NULL != retPtr)
	{
		for (size_t i = 0; i < 4; i++)
		{
			for (size_t t = 0; t < 4; t++)
			{
				retPtr->m[i][t] = mulps_sum_internal(
					ALIGN( left->colum[i]),ALIGN(right->colum[t]));
			}
		}
	}
	return retPtr;
}

SSGC_API __inline void __cdecl transpose4to2(matrix4map* source, matrix4map* target)
{
	__m128 t0 = _mm_shuffle_ps(ALIGN(source->colum[0]), ALIGN(source->colum[1]), _MM_SHUFFLE(1, 0, 1, 0));
	__m128 t2 = _mm_shuffle_ps(ALIGN(source->colum[2]), ALIGN(source->colum[3]), _MM_SHUFFLE(1, 0, 1, 0));

	target->colum[0] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(2, 0, 2, 0));
	target->colum[1] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(3, 1, 3, 1));
}

SSGC_API __inline void __cdecl transpose4to4(matrix4map* source, matrix4map* target)
{
	__m128 t0 = _mm_shuffle_ps(ALIGN(source->colum[0]), ALIGN(source->colum[1]), _MM_SHUFFLE(1, 0, 1, 0));
	__m128 t1 = _mm_shuffle_ps(ALIGN(source->colum[0]), ALIGN(source->colum[1]), _MM_SHUFFLE(3, 2, 3, 2));
	__m128 t2 = _mm_shuffle_ps(ALIGN(source->colum[2]), ALIGN(source->colum[3]), _MM_SHUFFLE(1, 0, 1, 0));
	__m128 t3 = _mm_shuffle_ps(ALIGN(source->colum[2]), ALIGN(source->colum[3]), _MM_SHUFFLE(3, 2, 3, 2));

	target->colum[0] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(2, 0, 2, 0));
	target->colum[1] = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(3, 1, 3, 1));
	target->colum[2] = _mm_shuffle_ps(t1, t3, _MM_SHUFFLE(2, 0, 2, 0));
	target->colum[3] = _mm_shuffle_ps(t1, t3, _MM_SHUFFLE(3, 1, 3, 1));

}

SSGC_API __inline float __cdecl det_mat4(matrix4* matPtr)
{
	__m128 cogeneration = _mm_set_ps(
		det_mat3_internal(matPtr->matrix.colum[1], matPtr->matrix.colum[2], matPtr->matrix.colum[3]),
		det_mat3_internal(matPtr->matrix.colum[0], matPtr->matrix.colum[2], matPtr->matrix.colum[3]),
		det_mat3_internal(matPtr->matrix.colum[0], matPtr->matrix.colum[1], matPtr->matrix.colum[3]),
		det_mat3_internal(matPtr->matrix.colum[0], matPtr->matrix.colum[1], matPtr->matrix.colum[2])
		);

	printf("%f\n\n", det_mat3_internal(matPtr->matrix.colum[1], matPtr->matrix.colum[2], matPtr->matrix.colum[3]));

	__m128 retvec = _mm_mul_ps((matPtr->transpose).colum[3],cogeneration);
	retvec = _mm_hsub_ps(retvec,retvec);
	retvec = _mm_hadd_ps(retvec,retvec);
	return -retvec.m128_f32[0];

}

SSGC_API __inline void __cdecl deinit_mat4(matrix4map* matPtr)
{
	_aligned_free(matPtr);
}