#include <sps2lib.h>
#include <sps2tags.h>
#include <sps2util.h>
#include <sps2regstructs.h>
#include "PS2Defines.h"
#include "ps2matrix4x4.h"
#include "ps2maths.h"
#include "sps2wrap.h"
#include "dma.h"
#include "pipeline.h"


// Singleton initialiser (note: Never make new copies of this classes anywhere else)
CPipeline CPipeline_SingletonInit;

#define ROTXMAX (PIHALF - 0.1f)

CPipeline::CPipeline()
{
	// Don't put anything in here which relies on other classes since
	// this class will probably be initialised first by the singleton 
	// instance.	
}

void CPipeline::Initialise(void)
{
	m_ViewProjection = Matrix4x4::IDENTITY;
	m_LightDirs = Matrix4x4::IDENTITY;
	m_LightCols = Matrix4x4::NULLMATRIX;
	m_Camera = Matrix4x4::IDENTITY;
	m_Projection = Matrix4x4::IDENTITY;
	m_Scale = Vector4(2048.0f, 2048.0f, ((float)0xFFFFFF) / 32.0f, 0.0f);
	
	m_near = 2.0f;				// Near plane
	m_far = 2000.0f;			// Far plane
	m_FOV = 60.0f;				// FOV
	m_Aspect = 4.0f / 3.0f;		// Aspect ratio
	
	m_ScreenW = sps2UScreenGetWidth();
	m_ScreenH = sps2UScreenGetHeight();
	
	assert((m_ScreenW > 0.0f) && (m_ScreenW > 0.0f));
	
	//printf("%f %f\n", m_ScreenW, m_ScreenH);
	
	m_CamPos = Vector4(0.0f, 0.0f, 0.0f, 1.0f);
	m_XRot = 0.0f;
	m_YRot = 0.0f;
	
	CalcProjection();
	
	m_ViewProjection = m_Camera * m_Projection;
	
	// Set ambient light on R=G=B=128
	m_LightCols(3,0) = 128.0f;
	m_LightCols(3,1) = 128.0f;
	m_LightCols(3,2) = 128.0f;
	
}

void CPipeline::CalcProjection(void)
{
	float FovYdiv2 = DegToRad(m_FOV) * 0.5f; 
	float cotFOV = 1.0f / (Sin(FovYdiv2) / Cos(FovYdiv2));
	
	// We will be projecting to the 4096 wide drawing area
	// Look also at the VU projection code
	float w = cotFOV * (m_ScreenW / 4096.0f) / m_Aspect;
	float h = cotFOV * (m_ScreenH / 4096.0f);

	m_Projection = Matrix4x4  (w,  0, 0,                                   0,
						 	   0, -h, 0,                                   0,
						 	   0,  0, (m_far+m_near) / (m_far-m_near),    -1,
						 	   0,  0, (2*m_far*m_near) / (m_far-m_near),   0);
						 
}


void CPipeline::Update(float Strafe, float Advance, float UpDown, float XRot, float YRot)
{
	// Process rotations
	m_YRot -= YRot * Abs(YRot) * 0.07f;
	m_XRot += XRot * Abs(XRot) * 0.07f;
	
	// Clamp xrot to prevent going upside down.
	if(m_XRot > ROTXMAX)
		m_XRot = ROTXMAX;
	
	if(m_XRot < -ROTXMAX)
		m_XRot = -ROTXMAX;
	
	// Sort out the rotations
	Matrix4x4 roty, rotx;
	roty.RotationY( m_YRot );
	rotx.RotationX( m_XRot );
	Matrix4x4 CamRot = rotx * roty;
	
	// Transpose rotation matrix to get camera matrix
	CamRot = Transpose(CamRot);

	// Get the direction vectors for the camera
	Vector4 vXAxis = Vector4(CamRot(0,0), CamRot(1,0), CamRot(2,0), 1);
	Vector4 vYAxis = Vector4(CamRot(0,1), CamRot(1,1), CamRot(2,1), 1);
	Vector4 vZAxis = Vector4(CamRot(0,2), CamRot(1,2), CamRot(2,2), 1);
	
	// Process movement
	Strafe *=   0.5f;
	Advance *=  0.5f;
	UpDown *=   0.5f;

	// Work out the requested move for this frame
	Vector4 vReqMove;
	vReqMove =  vXAxis * Strafe;
	vReqMove += vYAxis * UpDown;
	vReqMove += vZAxis * Advance;
	
	// And add it to the camera position
	m_CamPos += vReqMove;

	// Finally build the camera matrix
	Matrix4x4 matView;
	matView.Translation(-m_CamPos.x, -m_CamPos.y, -m_CamPos.z);
	
	m_Camera = matView * CamRot;
	m_ViewProjection = m_Camera * m_Projection;

}

void CPipeline::PositionCamera(const Vector4 & Position, const float XRot, const float YRot)
{
	m_CamPos = Position;
	m_YRot = YRot;
	m_XRot = XRot;
}

	
void CPipeline::SetLight1(const Vector4 & Direction, const Vector4 & Colour)
{
	// Set the direction
	Vector4 Temp = Direction;
	assert(Temp.LengthSqr() > 0.0f);
	Temp.NormaliseSelf();
	m_LightDirs(0,0) = Temp.x;
	m_LightDirs(1,0) = Temp.y;
	m_LightDirs(2,0) = Temp.z;

	
	// Set the colour
	m_LightCols(0,0) = Colour.x;
	m_LightCols(0,1) = Colour.y;
	m_LightCols(0,2) = Colour.z;
}

void CPipeline::SetLight2(const Vector4 & Direction, const Vector4 & Colour)
{
	// Set the direction
	Vector4 Temp = Direction;
	assert(Temp.LengthSqr() > 0.0f);
	Temp.NormaliseSelf();
	m_LightDirs(0,1) = Temp.x;
	m_LightDirs(1,1) = Temp.y;
	m_LightDirs(2,1) = Temp.z;

	
	// Set the colour
	m_LightCols(1,0) = Colour.x;
	m_LightCols(1,1) = Colour.y;
	m_LightCols(1,2) = Colour.z;
}

void CPipeline::SetLight3(const Vector4 & Direction, const Vector4 & Colour)
{
	// Set the direction
	Vector4 Temp = Direction;
	assert(Temp.LengthSqr() > 0.0f);
	Temp.NormaliseSelf();
	m_LightDirs(0,2) = Temp.x;
	m_LightDirs(1,2) = Temp.y;
	m_LightDirs(2,2) = Temp.z;

	
	// Set the colour
	m_LightCols(2,0) = Colour.x;
	m_LightCols(2,1) = Colour.y;
	m_LightCols(2,2) = Colour.z;
}

void CPipeline::SetAmbient(const Vector4 & Colour)
{
	// Set the colour
	m_LightCols(3,0) = Colour.x;
	m_LightCols(3,1) = Colour.y;
	m_LightCols(3,2) = Colour.z;

	//m_LightDirs.DumpMatrix4x4();
	//m_LightCols.DumpMatrix4x4();
}