/*
 * $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;
	return;
}

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 = &(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);
	   }
}