/*
 * $Id: altivec_math.c 200 2007-11-03 11:53:08Z hderuiter $
 *
 * $Date: 2007-11-03 12:26:08 $
 * $Revision: 000 $
 *
 * (C) 2009 by Hans de Ruiter
 * All rights reserved
 *
 * This file is part of the MiniGL library project
 * See the file Licence.txt for more details
 *
 */

#include "displaylists.h"
#include "sysinc.h"
#include <math.h>
#include <mgl/matrix.h>

#include "altivec_math.h"
#include <altivec.h>


extern BOOL useVectorUnit;

void disableDenormalizedException()
{
	// Need to turn off exceptions for denormalized numbers (e.g., the result of a div by zero)
	// Blender crashes without this
	// bit 15
	vector unsigned short dnExceptionOffMask = (vector unsigned short){0,0,0,0,0,0,0x1,0};
	vector unsigned short vscrVal;

	vector unsigned short oldVSCRVal = (vector unsigned short) vec_mfvscr ();

	vscrVal = vec_or(oldVSCRVal, dnExceptionOffMask);
	vec_mtvscr(vscrVal);
}

void m_Mult_altivec(Matrix *pA, float *v, int vtype, Matrix *pC)
{
	// http://www.freevec.org/function/matrix_4x4_multiplication_floats
	vector float zero;
	vector float vA1, vA2, vA3, vA4, vB1, vB2, vB3, vB4;
	vector float vC1, vC2, vC3, vC4;
	zero = (vector float)vec_splat_u32(0);

	if (((int)&(pA->v) & 0x0000000f) == 0) {
		//IExec->DebugPrintF("Loading aligned: %lX\n", (unsigned int)&pA->v);
		LOAD_ALIGNED_MATRIX(&(pA->v), vB1, vB2, vB3, vB4)
	}
	else {
		//IExec->DebugPrintF("Loading unaligned: %lX\n", (unsigned int)&pA->v);
		LOAD_UNALIGNED_MATRIX(&(pA->v), vB1, vB2, vB3, vB4);
	}

	if (((int)v & 0x0000000f) == 0) {
		//IExec->DebugPrintF("Loading aligned: %lX\n", (unsigned int)v);
		LOAD_ALIGNED_MATRIX(v, vA1, vA2, vA3, vA4)
	}
	else {
		//IExec->DebugPrintF("Loading unaligned: %lX\n", (unsigned int)v);
		LOAD_UNALIGNED_MATRIX(v, vA1, vA2, vA3, vA4)
	}

	vC1 = vec_madd( vec_splat(vA1, 0), vB1, zero);
	vC2 = vec_madd( vec_splat(vA2, 0), vB1, zero);
	vC3 = vec_madd( vec_splat(vA3, 0), vB1, zero);
	vC4 = vec_madd( vec_splat(vA4, 0), vB1, zero);
	vC1 = vec_madd( vec_splat(vA1, 1), vB2, vC1);
	vC2 = vec_madd( vec_splat(vA2, 1), vB2, vC2);
	vC3 = vec_madd( vec_splat(vA3, 1), vB2, vC3);
	vC4 = vec_madd( vec_splat(vA4, 1), vB2, vC4);
	vC1 = vec_madd( vec_splat(vA1, 2), vB3, vC1);
	vC2 = vec_madd( vec_splat(vA2, 2), vB3, vC2);
	vC3 = vec_madd( vec_splat(vA3, 2), vB3, vC3);
	vC4 = vec_madd( vec_splat(vA4, 2), vB3, vC4);
	vC1 = vec_madd( vec_splat(vA1, 3), vB4, vC1);
	vC2 = vec_madd( vec_splat(vA2, 3), vB4, vC2);
	vC3 = vec_madd( vec_splat(vA3, 3), vB4, vC3);
	vC4 = vec_madd( vec_splat(vA4, 3), vB4, vC4);

	STORE_ALIGNED_MATRIX(&(pC->v), vC1, vC2, vC3, vC4);
	//m_PrintMatrix(pC);
	pC->flags = 0;
}

void v_TransformToClip(GLcontext context)
{
	int i;
	vector float vm1, vm2, vm3, vm4;
	vector float zero;
	vector float vec_1, vec_2, vec_3, vec_4;
	float* mat_ptr = (float*)&(context->CombinedMatrix.v);
	if (((int)mat_ptr) & 0x0000000f) {
		LOAD_UNALIGNED_MATRIX(mat_ptr, vm1, vm2, vm3, vm4)
	}
	else {
		LOAD_ALIGNED_MATRIX(mat_ptr, vm1, vm2, vm3, vm4)
	}
	zero = (vector float)vec_splat_u32(0);
	int counter = context->VertexBufferPointer;
	MGLVertex *vf = context->VertexBuffer;
	for (i = 0; i < counter; i++) {
		MGLVertex *v = &vf[i];
		vec_1 = vec_splat(v->object_vec, 0);
		vec_2 = vec_splat(v->object_vec, 1);
		vec_3 = vec_splat(v->object_vec, 2);
		vec_4 = vec_splat(v->object_vec, 3);
			v->clip_vec = vec_madd(vm1, vec_1, zero);
		v->clip_vec = vec_madd(vm2, vec_2, v->clip_vec);
		v->clip_vec = vec_madd(vm3, vec_3, v->clip_vec);
		v->clip_vec = vec_madd(vm4, vec_4, v->clip_vec);
	}
}