////////////////////////////////////////////////////////////////////////////////
//
// Copyright 2016 RWS Inc, All Rights Reserved
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of version 2 of the GNU General Public License as published by
// the Free Software Foundation
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
//
////////////////////////////////////////////////////////////////////////////////
//
// VectorMath.h
// Project: RSPiX
//
// This module implements high speed manipulations of standard mathematical
// vectors and matrices.
//
// History:
//		??/??/?? JRD	Started.
//
//		02/11/97	JMI	Added Load() and Save() for compatability with RChannel,
//							RResMgr, and other such file-based thingers.
//
////////////////////////////////////////////////////////////////////////////////
#ifndef VECTOR_MATH_H
#define VECTOR_MATH_H

#include "System.h"
#ifdef PATHS_IN_INCLUDES
	#include "ORANGE/File/file.h"
	#include "ORANGE/QuickMath/QuickMath.h"
	#include "ORANGE/QuickMath/sh4_math.h"
#else
	#include "file.h"
	#include "quickmath.h"
#endif

//===================================
// This file contains data and operations
// for high speed manipulations of standard
// mathematical vectors and matrices.

/*===================================
RP3d; // This is a 3d point.
inline void rspMakeHom(RP3d& p) // do if in question
inline void rspCopy(RP3d& a,RP3d& b)
inline void rspMakeUnit(RP3d& p)
inline REAL rspDot(RP3d& a,RP3d& b)
inline void rspCross(RP3d& a,RP3d& b,RP3d& c) // c = a x b
RTransform 
	{ T[16],
	RTransform() // init to an identity transform
	RTransform(REAL* M) // init to a copy of another transform
	void Make1()
	void Make0()
	void PreMulBy(REAL* M)
	void Mul(REAL* A,REAL* B) // 4x4 transforms:
	void Transform(RP3d &p)
	void TransformInto(RP3d &XF, RP3d& p) XF = this x p
	void Trans(REAL x,REAL y,REAL z)
	void Scale(REAL a,REAL b, REAL c)
	void Rz(short sDeg) // CCW!
	void Rx(short sDeg) // CCW!
	void Ry(short sDeg) // CCW!
	void MakeScreenXF(
		REAL x1,REAL y1,REAL w1,REAL h1,
		REAL x2,REAL y2,REAL w2,REAL h2)
	void MakeRotTo(RP3d point,RP3d up)
	void MakeRotFrom(RP3d point,RP3d up)
	}
//=================================*/

#define REAL float // float conserves internal memory..
// Note that if REAL is double, than it should somehow hook
// double sized quick trig, so it doesn't have to do
// conversions each time!

// This is an aggregate type used with RTransform
typedef union
	{
	// 06/30/97 MJR - For the metrowerks compiler, initializations don't
	// seem to work right unless the array comes before the struct!
	REAL v[4];
	struct
		{
		REAL x;
		REAL y;
		REAL z;
		REAL w;
		};

	int16_t Load(RFile* pfile)
		{ return pfile->Read(v, 4) != 4; }

	} RP3d; // This is a 3d point.

inline int operator==(const RP3d& lhs, const RP3d& rhs)
	{
	if (lhs.v == rhs.v)
		return 1;
	return 0;
	}


// a -= b;
inline void rspSub(RP3d& a,RP3d& b)
	{
	a.x -= b.x;
	a.y -= b.y;
	a.z -= b.z;
	}


// And some useful constants for manipulation:
const int16_t ROW[4] = {0,4,8,12};
const int16_t ROW0 = 0;
const int16_t ROW1 = 4;
const int16_t ROW2 = 8;
const int16_t ROW3 = 12;

const REAL Identity[16] = { 1.0, 0.0, 0.0, 0.0,
							0.0, 1.0, 0.0, 0.0,
							0.0, 0.0, 1.0, 0.0,
							0.0, 0.0, 0.0, 1.0};

// NOW, the class based transform allows matrix
// multiplication to occur WITHOUT multiplying
// 2 matrices together.  This prevents a malloc
// nightmare:
//
class RTransform
	{
public:
	
	REAL T[16]  __attribute__ ((aligned (32))); // This is compatible with the aggregate transform	
	RTransform() // init to an identity transform
		{ 
		for (int16_t i=0;i<16;i++) 
			T[i]=Identity[i];
		}

	RTransform(REAL* M) // init to a copy of another transform
		{ 
		for (int16_t i=0;i<16;i++) 
			T[i]=M[i];
		}

	~RTransform(){};

	int operator==(const RTransform& rhs) const
		{
		if (T == rhs.T)
			return 1;
		return 0;
		}

	void Make1() // identity matrix
		{
		for (int16_t i=0;i<16;i++) 
			T[i]=Identity[i];
		}

	void Make0() // null matrix
		{
		for (int16_t i=0;i<15;i++) 
			T[i]=(REAL)0;
		T[15] = (REAL)1;
		}

	//------------------------
	// ALL TRANSFORMATIONS ARE PRE-MULTIPLIES,
	// A Partial transform, assuming R3 = {0,0,0,1};
	// 
	void PreMulBy(REAL* M)
	{
		mat_load((matrix_t *)&(*T));
		mat_apply((matrix_t *)&(*M));
		mat_store((matrix_t *)&(*T));
	}

	// Oversets the current Transform with the resultant!
	// = A * B
	void Mul(REAL* A,REAL* B) // 4x4 transforms:
	{
		mat_load((matrix_t *)&(*B));
		mat_apply((matrix_t *)&(*A));
		mat_store((matrix_t *)&(*T));
	}

	// todo:
	float t[16];
	void UpdateMatr()
	{				
		t[0] = T[0];
		t[1] = T[4];
		t[2] = T[8];
		t[3] = T[12];

		t[4] = T[1];
		t[5] = T[5];
		t[6] = T[9];
		t[7] = T[13];

		t[8] = T[2];
		t[9] = T[6];
		t[10] = T[10];
		t[11] = T[14];

		t[12] = T[3];
		t[13] = T[7];
		t[14] = T[11];
		t[15] = T[15];	

		float *tt = t;

        __asm__ volatile(
			" fschg\n" 
 			" fmov   @%0+,xd0\n" 
			" fmov   @%0+,xd2\n" 
			" fmov   @%0+,xd4\n" 
			" fmov   @%0+,xd6\n" 
			" fmov   @%0+,xd8\n" 
			" fmov   @%0+,xd10\n" 
			" fmov   @%0+,xd12\n" 
			" fmov   @%0+,xd14\n" 
			" fschg\n" 
			: "+r"(tt) 
		);					
	}

	// Transform an actual point ( overwrites old point )
	// Doex a premultiply!
	void Transform(RP3d &p)
	{	
		register float __x __asm__("fr12") = (p.v[0]);
        register float __y __asm__("fr13") = (p.v[1]);
        register float __z __asm__("fr14") = (p.v[2]);
		register float __w __asm__("fr15") = (p.v[3]);

		__asm__ __volatile__(
        	"ftrv  xmtrx, fv12\n"
            : "=f" (__x), "=f" (__y), "=f" (__z), "=f" (__w)
            : "0"  (__x), "1"  (__y), "2"  (__z) , "3" (__w) 
		);

		p.v[0] = __x;
		p.v[1] = __y;
		p.v[2] = __z;
		p.v[3] = 1.0;
	}

	// Transform an actual point, and places the answer into a different pt
	// Doex a premultiply!
	void TransformInto(RP3d& vSrc, RP3d& vDst)
	{	
		register float __x __asm__("fr12") = (vSrc.v[0]);
        register float __y __asm__("fr13") = (vSrc.v[1]);
        register float __z __asm__("fr14") = (vSrc.v[2]);
		register float __w __asm__("fr15") = (vSrc.v[3]);

		__asm__ __volatile__(
        	"ftrv  xmtrx, fv12\n"
            : "=f" (__x), "=f" (__y), "=f" (__z), "=f" (__w)
            : "0"  (__x), "1"  (__y), "2"  (__z) , "3" (__w) 
		);

		vDst.v[0] = __x;
		vDst.v[1] = __y;
		vDst.v[2] = __z;
		vDst.v[3] = 1.0;
	}

	// Assumes R3 = {0,0,0,1}
	void Trans(REAL x,REAL y,REAL z)
		{
		T[ROW0+3] += x;
		T[ROW1+3] += y;
		T[ROW2+3] += z;
		}

	void Scale(REAL a,REAL b, REAL c)
		{
		for (int16_t i=0;i<4;i++)
			{
			T[ ROW0 + i] *= a;	
			T[ ROW1 + i] *= b;	
			T[ ROW2 + i] *= c;	
			}
		}

	void Rz(int16_t sDeg) // CCW!
		{
			mat_load((matrix_t *)&T);			
			mat_rotate_z(float(sDeg) * rspDegToRad);
			mat_store((matrix_t *)&T);
		}

	void Rx(int16_t sDeg) // CCW!
		{
			mat_load((matrix_t *)&T);			
			mat_rotate_x(float(sDeg) * rspDegToRad);
			mat_store((matrix_t *)&T);
		}

	void Ry(int16_t sDeg) // CCW!
		{
			mat_load((matrix_t *)&T);			
			mat_rotate_y(float(sDeg) * rspDegToRad);
			mat_store((matrix_t *)&T);
		}

	// a 3d ORTHOGONAL mapping from REAL box1 to box2
	// useful in screen and orthogonal view xforms
	// Use rspSub to create w vertices (w,h,d)
	// x1 BECOMES x2.  Note that w1 must NOT have any 0's.
	//
	void MakeBoxXF(RP3d &x1,RP3d &w1,RP3d &x2,RP3d &w2)
		{
		// NOT OF MAXIMUM SPEED!
		Make1();
		Trans(-x1.x,-x1.y,-x1.z);
		Scale(w2.x/w1.x,w2.y/w1.y,w2.z/w1.z);
		Trans(x2.x,x2.y,x2.z);
		}


	// Loads instance data for this Transform from the specified
	// file.
	int16_t Load(				// Returns 0 on success.
		RFile*	pfile)	// In:  Ptr to file to load from.  Must be open with
								// read access.
		{
		// Read the entire matrix in one kerchunk.  Bang!  I mean Kerchunk!
		pfile->Read(T, sizeof(T) / sizeof(T[0]) );
		// Success can be measured in terms of I/O errors.
		// Also, we should not have read past the end yet.
		return pfile->Error() | pfile->IsEOF();
		}

	};


//===================================
#endif
