#include "Camera.h"
#include "Lights.h"
#include <iostream>

Camera* Camera::instance;

#define TWO_PI (float)D3DX_PI*2.0f
#define DEG2RAD( a ) (float)( a * D3DX_PI / 180.f )
#define MOVESENS 50.0f
#define MOUSESENS 0.25f

Camera::Camera(void)
{
	diManager = DiManager::getInstance();
	recordInterval = 1.f; 
	isRecording = false; 
	lastStateRKey = lastStatePKey = lastStateF6Key = lastStateF9Key = false;  
	isCameraControlled = true;
	
	clearVectors = true; 
    timeStep = 1.f;
	record_index = 0; 
    timeElapsed = curTime = timeSinceControlling = 0.f;
    util = Util::getInstance(); 
	forwardAccelerator = rightAccelerator = 1.f; 
	upVector = D3DXVECTOR3(0.0057018f, 0.997858f, 0.0651615f);
	position = D3DXVECTOR3(74.1989f, 8.35286f, -770.319f);
	lookAt = D3DXVECTOR3(74.2859f, 8.28745f, -769.325f);
	defaultView = D3DXVECTOR3(0.0f, 0.0f, -1.0f);
	defaultUp = D3DXVECTOR3(0.0f, 1.0f, 0.0f);
	yaw = 0.0;
	pitch = 0.0;
	roll = 0.0f;
	zNear = 1.0f;
	zFar = 10000.0f;
	fov = D3DX_PI * 0.45f;
	
	mousePosition.x = 0;
	mousePosition.y = 0;

	if(isCameraControlled)
		fetchCameraData();
}


void Camera::update(float dt)
{
	mousePosition = diManager->getMouseMovement();

	pitch -= DEG2RAD(mousePosition.y * MOUSESENS) ;
	
	yaw -= DEG2RAD(mousePosition.x * MOUSESENS) ;
	if(yaw > TWO_PI)
		yaw -= TWO_PI;
	else
	if(yaw < 0)
		yaw = TWO_PI+yaw;

	if(pitch > TWO_PI)
		pitch -= TWO_PI;
	else
	if(pitch < 0)
		pitch = TWO_PI+pitch;
	
	checkCameraKeys();

	position += strafeLeftRightDir*rightVector* dt*MOVESENS + moveForwardBackDir*forwardVector* dt*MOVESENS + upVector*moveUpDownDir*dt*MOVESENS ;
	
	D3DXMatrixRotationYawPitchRoll( &rotationMatrix, yaw, pitch, roll );

	D3DXVec3TransformCoord( &lookAt, &defaultView, &rotationMatrix );
	D3DXVec3TransformCoord( &upVector, &defaultUp, &rotationMatrix );

	D3DXVec3Normalize( &forwardVector, &lookAt );
	D3DXVec3Cross( &rightVector, &lookAt, &upVector );
	D3DXVec3Normalize( &rightVector, &rightVector );

	lookAt = position + lookAt;

	

	if(isCameraControlled) {
		controlCamera(&timeSinceControlling); 
		timeSinceControlling += dt; 
	}

	D3DXMatrixLookAtRH( &viewMatrix, &position, &lookAt, &upVector );

	buildFrustum();

	if(isRecording) {
		//first frame to capture
		if(curTime == 0) {
			record_time.push_back(0.f); 
			vector<D3DXVECTOR3> temp_data; 
			temp_data.push_back(position); 
			temp_data.push_back(lookAt); 
			temp_data.push_back(upVector); 
			record_data.push_back(temp_data); 

		}

		curTime += dt;

		if(timeElapsed >= recordInterval) {
			record_time.push_back(curTime); 
			vector<D3DXVECTOR3> temp_data; 
			temp_data.push_back(position); 
			temp_data.push_back(lookAt); 
			temp_data.push_back(upVector); 
			record_data.push_back(temp_data); 
			timeElapsed = 0;
		} else {
			timeElapsed += dt;
		}
	}

	
}

void Camera::updateProjection(float aspect)
{
	 aspectR = aspect;
	 D3DXMatrixPerspectiveFovRH( &projectionMatrix,fov , aspect, zNear, zFar );
}

D3DXMATRIX* Camera::getViewMatrix()
{
	return &viewMatrix;
}

void Camera::setLookAt(D3DXVECTOR3 look)
{
}
void Camera::setPosition(D3DXVECTOR3 pos)
{
}
void Camera::setUpVector(D3DXVECTOR3 pos)
{
}

void Camera::strafeLeftRight(float x)
{
	strafeLeftRightDir = x;
}
void Camera::moveForwardBack(float z)
{
	moveForwardBackDir += z;
}

void Camera::moveUpDown(float y)
{
	moveUpDownDir += y;
}

void Camera::setMousePosition(int x, int y)
{
	mousePosition = D3DXVECTOR2((float)x,(float)y);
}

void Camera::checkCameraKeys()
{	
	if(diManager->isKeyPressed(DIK_A))
		//strafeLeftRightDir = -1.0f;
		strafeLeftRightDir = -rightAccelerator; 
	else
	{
		if(diManager->isKeyPressed(DIK_D))
			strafeLeftRightDir = rightAccelerator; 
		else
			strafeLeftRightDir = 0.f; 
	}	

	if(diManager->isKeyPressed(DIK_W))
		moveForwardBackDir = forwardAccelerator;
	else
	{
		if(diManager->isKeyPressed(DIK_S))
			moveForwardBackDir = -forwardAccelerator;
		else
			moveForwardBackDir = 0.0f;
	}

	if(diManager->isKeyPressed(DIK_C))
		moveUpDownDir = -0.2f;
	else
	{
		if(diManager->isKeyPressed(DIK_SPACE))
			moveUpDownDir = 0.2f;
		else
			moveUpDownDir = 0.0f;
	}

	//pressed R
	 if(diManager->isKeyPressed(DIK_R)){
        lastStateRKey = true;

    //key not pressed. Check if it was pressed last frame (onKeyRelease)     
    } else {
        
        if (lastStateRKey) {
            //switch states
			if(isRecording) {
                stopRecording();
				cout << "Stopped recording" << endl;
			} else {
                startRecording();
				cout << "Start recording" << endl;
			}
        }

        lastStateRKey = false;
    }
	
	 //pressed P
	 if(diManager->isKeyPressed(DIK_P)) {
		 lastStatePKey = true;
		 
	 } else {
		 if(lastStatePKey) {

			 isCameraControlled = !isCameraControlled;
			 if(isCameraControlled) {
				 cout << "Start camera movement" << endl; 
				 if(isRecording) {
					cout << "Canīt control camera while recording!" << endl;
					isCameraControlled = false; 
				 } else {
					fetchCameraData();
				}
			 }
		 }


		 lastStatePKey = false; 
	 }

    //isCameraControlled should be set to false after the automatic movement finished
	 
	if(diManager->isKeyPressed(DIK_END) && isCameraControlled)
		isCameraControlled = false; 

	if(diManager->isKeyPressed(DIK_LSHIFT)) {
		forwardAccelerator += 0.04f; 
		if(forwardAccelerator > 8.f)
			forwardAccelerator = 8.f; 
	}
	if (diManager->isKeyPressed(DIK_LCONTROL)){
		forwardAccelerator -= 0.04f;
		if(forwardAccelerator < 0.f)
			forwardAccelerator = 0.f; 
	}
	if (diManager->isKeyPressed(DIK_Q)){
		rightAccelerator  -= 0.04f; 
		if(rightAccelerator < 0.f)
			rightAccelerator = 0.f; 
	}
	if (diManager->isKeyPressed(DIK_E)){
		rightAccelerator += 0.04f;
		if (rightAccelerator > 8.f)
			rightAccelerator = 8.f; 
	}

	if (diManager->isKeyPressed(DIK_F6)) {
		// record just one position etc
		lastStateF6Key = true; 
	} else {
		if(lastStateF6Key) {
			if(clearVectors) {
				record_time.clear(); 
				record_data.clear(); 
				clearVectors = false;
			}

			vector<D3DXVECTOR3> temp_data; 
			temp_data.push_back(position); 
			temp_data.push_back(lookAt); 
			temp_data.push_back(upVector); 
			record_data.push_back(temp_data);
			record_time.push_back((int)record_data.size()-1*0.5f);
		}

		lastStateF6Key = false; 
	}

	if (diManager->isKeyPressed(DIK_F9)) {
		lastStateF9Key = true; 
	} else {
		if(lastStateF9Key) {
			
			util->writeToFile(record_time, record_data);
			clearVectors = true; 
		}

		lastStateF9Key = false;
		
	}

}

D3DXVECTOR3* Camera::getEyePosition()
{
	return &position;
}

D3DXMATRIX* Camera::getProjectionMatrix()
{
	return &projectionMatrix;
}

D3DXMATRIX* Camera::getLightViewMatrix()
{
	return &lightViewMatrix;
}

D3DXMATRIX* Camera::getLightProjectionMatrix()
{
	return &lightProjectionMatrix;
}

void Camera::buildLightViewAndProjection()
{
	D3DXMatrixLookAtRH( &lightViewMatrix, &(D3DXVECTOR3(7.77775f, 245.699f, 193.521f)), &(D3DXVECTOR3(7.77775f, 244.802f, 193.079f)), &(D3DXVECTOR3(0,1,0)) );
	D3DXMatrixPerspectiveFovRH( &lightProjectionMatrix,D3DX_PI * 0.45f , aspectR, zNear, 1000.f );
}

void Camera::buildFrustum()
{
	D3DXMATRIX viewProjection;
    D3DXMatrixMultiply( &viewProjection, &viewMatrix, &projectionMatrix );
    //D3DXMatrixMultiply( &viewProjection, &m_mView, &m_mProjection );
    // Left plane
    frustum[0].a = viewProjection._14 + viewProjection._11;
    frustum[0].b = viewProjection._24 + viewProjection._21;
    frustum[0].c = viewProjection._34 + viewProjection._31;
    frustum[0].d = viewProjection._44 + viewProjection._41;

    // Right plane
    frustum[1].a = viewProjection._14 - viewProjection._11;
    frustum[1].b = viewProjection._24 - viewProjection._21;
    frustum[1].c = viewProjection._34 - viewProjection._31;
    frustum[1].d = viewProjection._44 - viewProjection._41;

    // Top plane
    frustum[2].a = viewProjection._14 - viewProjection._12;
    frustum[2].b = viewProjection._24 - viewProjection._22;
    frustum[2].c = viewProjection._34 - viewProjection._32;
    frustum[2].d = viewProjection._44 - viewProjection._42;

    // Bottom plane
    frustum[3].a = viewProjection._14 + viewProjection._12;
    frustum[3].b = viewProjection._24 + viewProjection._22;
    frustum[3].c = viewProjection._34 + viewProjection._32;
    frustum[3].d = viewProjection._44 + viewProjection._42;

    // Near plane
    frustum[4].a = viewProjection._13;
    frustum[4].b = viewProjection._23;
    frustum[4].c = viewProjection._33;
    frustum[4].d = viewProjection._43;

    // Far plane
    frustum[5].a = viewProjection._14 - viewProjection._13;
    frustum[5].b = viewProjection._24 - viewProjection._23;
    frustum[5].c = viewProjection._34 - viewProjection._33;
    frustum[5].d = viewProjection._44 - viewProjection._43;

    // Normalize planes
    for ( int i = 0; i < 6; i++ )
    {
        D3DXPlaneNormalize( &frustum[i], &frustum[i] );
    }
}

bool Camera::testSphereInFrustum(BoundingSphere bs)
{
	for ( int i = 0; i < 6; i++ )
    {
        if ( D3DXPlaneDotCoord( &frustum[i], &bs.center ) + bs.radius < 0 )
        {
            return false;
        }
    }
    return true;
}

void Camera::startRecording() {
    isRecording = true;
	record_time.clear(); 
	record_data.clear(); 
    
}

void Camera::stopRecording() {
	util->writeToFile(record_time, record_data);
	isRecording = false;
    curTime = 0.f;
}

void Camera::fetchCameraData() {
	
	if(record_data.empty())
		util->getDataFromFile(&record_data, &record_time); 

	
}

void Camera::controlCamera(float* time) {


	//less than 3 interpolation points
	if((int)record_data.size() < 3) {
		cout << "Canīt interpolate camera movement with < 3 control points" << endl;

		// >3 control points

		//look if interpolation has finished already. 
	} else if(*time >= record_time.at(((int)record_time.size())-1)) {
		isCameraControlled = false;
		record_index = 0; 
		*time = 0; 
		cout << endl << "Movement finished." << endl; 
		//return
	} else {

		//interpolation not yet finished


		float weight; 

		//interpolation between p0, p0, p1, p2
		if(*time < record_time.at(1)) {
			weight = *time/(record_time.at(1)-record_time.at(0)); 
			interpolateCatMullRom(0, 0, 1, 2, weight); 
			//interpolation between p_n-2, p_n-1, p_n, p_n
		} else if(*time > record_time.at((int)record_time.size() -2)) {
			int size = (int)record_time.size(); 
			weight = (*time - record_time.at(size-2))/(record_time.at(size-1)-record_time.at(size-2)); 
			interpolateCatMullRom(size-3, size-2, size-1, size-1, weight); 

		} else {
			
			if(*time > record_time.at(record_index+1) && record_index < (int)record_time.size()-1)
				record_index++; 

			weight = (*time - record_time.at(record_index))/(record_time.at(record_index+1) - record_time.at(record_index));
			interpolateCatMullRom(record_index-1, record_index, record_index+1, record_index+2, weight); 
		}

	}
}

void Camera::interpolateCatMullRom(int p0, int p1, int p2, int p3, float s) {

	    D3DXVec3CatmullRom(&position, &record_data.at(p0).at(0), &record_data.at(p1).at(0), &record_data.at(p2).at(0), &record_data.at(p3).at(0), s);
		D3DXVec3CatmullRom(&lookAt, &record_data.at(p0).at(1), &record_data.at(p1).at(1), &record_data.at(p2).at(1), &record_data.at(p3).at(1), s);
		D3DXVec3CatmullRom(&upVector, &record_data.at(p0).at(2), &record_data.at(p1).at(2), &record_data.at(p2).at(2), &record_data.at(p3).at(2), s);
}