#include "rtsCameraController.h"
#include <math.h>
rtsCamera::rtsCamera()
{
	m_camera_state.position = point3D<float>(10, 10, 10);
	m_camera_state.lookat = point3D<float>(0, 0, 0);
	m_camera_state.up = vector3D<float>(0, 1, 0);
	m_camera_state.pers_view_angle = 60;
	m_camera_state.near_plane = 1;
	m_camera_state.far_plane = 100;
}

rtsCamera::~rtsCamera()
{

}

rtsCamera::rtsCamera(rtsCameraState initial_state)
{
	m_camera_state = initial_state;

	//make sure that the view and lookat vectors are orthogonal
	vector3D<float> lookat = m_camera_state.lookat - m_camera_state.position;
	vector3D<float> up = m_camera_state.up;
	vector3D<float> side = lookat.X(up);
	up = side.X(lookat);
	up.Normalize();
	m_camera_state.up = up;
}

rtsCameraState rtsCamera::getState()
{
	return m_camera_state;
}

vector3D<float> rtsCamera::getViewVector()
{
	vector3D<float> result = m_camera_state.lookat - m_camera_state.position;
	result.Normalize();
	return result;
}

vector3D<float> rtsCamera::getUpVector()
{
	return m_camera_state.up;
}

point3D<float> rtsCamera::getPosition()
{
	return m_camera_state.position;
}

void rtsCamera::setState(rtsCameraState camera_state)
{
	m_camera_state = camera_state;
	
	//re-orthogonalize the vectors
	vector3D<float> view = m_camera_state.lookat - m_camera_state.position;
	vector3D<float> side = view.X(m_camera_state.up);
	m_camera_state.up = side.X(view);
	m_camera_state.up.Normalize();
}

void rtsCamera::LookAt(point3D<float> point)
{
	//looks at a point

	//find the new view vector
	vector3D<float> view = point - m_camera_state.position;

	//normalize the view vector
	view.Normalize();

	//prepare a new side vector and up vector
	vector3D<float> side;
	vector3D<float> up;

	//get the up vector
	//if the new viewvector is at 0 or 180 degrees to the up vector
	float cos_angle = view*m_camera_state.up;
	if(cos_angle == 1.0f || cos_angle == -1.0f)
	{
		//re-calculate the up vector
		up = m_camera_state.up.X(m_camera_state.lookat - m_camera_state.position);
	}
	else
	{
		//otherwise, just get the current up vector
		up = m_camera_state.up;
	}
	

	//correct the up vector based on the new view vector
	side = up.X(view);
	up = view.X(side);
	up.Normalize();

	//change the camera state
	m_camera_state.up = up;
	m_camera_state.lookat = point;
}

void rtsCamera::Position(point3D<float> p)
{
	m_camera_state.position = p;
}

void rtsCamera::Up(vector3D<float> up)
{
	m_camera_state.up = up;
}

void rtsCamera::DollyPosition(point3D<float> p)
{
	vector3D<float> adjustment = p-m_camera_state.position;
	m_camera_state.position = p;
	m_camera_state.lookat = m_camera_state.lookat + adjustment;
}

point3D<float> rtsCamera::getLookAtPoint()
{
	return m_camera_state.lookat;
}



void rtsCamera::Pan(double x, double y)
{
	//first calculate the lookat and side vectors
	vector3D<float> lookatvector=m_camera_state.lookat - m_camera_state.position;
	vector3D<float> sidevector = lookatvector.X(m_camera_state.up);
	sidevector.Normalize();

	m_camera_state.position=m_camera_state.position+sidevector*x;
	m_camera_state.lookat=m_camera_state.lookat+sidevector*x;

	vector3D<float> upvector = lookatvector.X(sidevector);
	upvector.Normalize();
	m_camera_state.position=m_camera_state.position+upvector*y;
	m_camera_state.lookat=m_camera_state.lookat+upvector*y;
}

void rtsCamera::RotateUpDown(double degrees)
{
	//first calculate the lookat and side vectors
	vector3D<float> lookatvector=m_camera_state.lookat-m_camera_state.position;
	vector3D<float> sidevector = lookatvector.X(m_camera_state.up);
	m_camera_state.up=sidevector.X(lookatvector);
	m_camera_state.up.Normalize();
	sidevector.Normalize();

	//translate the look-at point to the origin (and the camera with it)
	point3D<float> origin = point3D<float>(0.0, 0.0, 0.0);
	vector3D<float> translateCamera = origin-m_camera_state.lookat;

	point3D<float> translatedCamera=m_camera_state.position+translateCamera;

	//the next step is to rotate the side vector so that it lines up with the z axis
	double a=sidevector.x;
	double b=sidevector.y;
	double c=sidevector.z;

	double d=sqrt(b*b + c*c);

	//error correction for when we are already looking down the z-axis
	if(d==0)
		return;

	vector3D<float> XZplane = vector3D<float>(translatedCamera.x, 
								(translatedCamera.y*c/d - translatedCamera.z*b/d), 
								(translatedCamera.y*b/d + translatedCamera.z*c/d));

	vector3D<float> Zaxis = vector3D<float>(XZplane.x*d - XZplane.z*a,
								XZplane.y,
								XZplane.x*a + XZplane.z*d);

	vector3D<float> rotated = vector3D<float>(Zaxis.x*cos(TORADIANS(degrees)) - Zaxis.y*sin(TORADIANS(degrees)),
								Zaxis.x*sin(TORADIANS(degrees)) + Zaxis.y*cos(TORADIANS(degrees)),
								Zaxis.z);

	vector3D<float> XZout = vector3D<float>( rotated.x*(d/(a*a + d*d)) + rotated.z*(a/(a*a + d*d)),
								rotated.y,
								rotated.x*(-a/(a*a+d*d)) + rotated.z*(d/(a*a + d*d)));

	vector3D<float> result = vector3D<float>( XZout.x,
								XZout.y*(c*d/(b*b + c*c)) + XZout.z*(b*d/(b*b + c*c)),
								XZout.y*(-b*d/(b*b + c*c)) + XZout.z*(c*d/(b*b + c*c)));

	result=result-translateCamera;

	m_camera_state.position.x=result.x;
	m_camera_state.position.y=result.y;
	m_camera_state.position.z=result.z;

}

void rtsCamera::Yaw(double degrees)
{
	//basically, we have to rotate the look-at point around the up vector
	//first, translate the look-at point so that the camera is at the origin
	point3D<float> origin(0.0, 0.0, 0.0);
	point3D<float> temp_lookat = m_camera_state.lookat - (m_camera_state.position - origin);
	
	//create a rotation matrix to rotate the lookat point around the up vector
	float x=m_camera_state.up.x;
	float y=m_camera_state.up.y;
	float z=m_camera_state.up.z;
	float c=cos(TORADIANS(-degrees));
	float s=sin(TORADIANS(-degrees));
	float t=1.0 - cos(TORADIANS(-degrees));
	float m00 = t*x*x + c;
	float m01 = t*x*y + s*z;
	float m02 = t*x*z - s*y;
	float m03 = 0;
	float m10 = t*x*y - s*z;
	float m11 = t*y*y + c;
	float m12 = t*y*z + s*x;
	float m13 = 0;
	float m20 = t*x*z + s*y;
	float m21 = t*y*z - s*x;
	float m22 = t*z*z + c;
	float m23 = 0;
	float m30 = 0;
	float m31 = 0;
	float m32 = 0;
	float m33 = 1;
	matrix4x4<float> rotation(m00, m01, m02, m03,
					   m10, m11, m12, m13,
					   m20, m21, m22, m23,
					   m30, m31, m32, m33);
	point3D<float> result = rotation*temp_lookat + (m_camera_state.position - origin);
	m_camera_state.lookat = result;
}

void rtsCamera::Pitch(double degrees)
{
	//basically, we have to rotate the look-at point and up vector around the side vector
	//first, translate the look-at point so that the camera is at the origin
	point3D<float> origin(0.0, 0.0, 0.0);
	
	//find all three necessary vectors
	vector3D<float> temp_lookat = m_camera_state.lookat - m_camera_state.position;
	double lookat_length = temp_lookat.Length();
	vector3D<float> temp_up = m_camera_state.up;
	vector3D<float> temp_side = temp_lookat.X(temp_up);
	temp_lookat.Normalize();
	temp_up.Normalize();
	temp_side.Normalize();


	//create a rotation matrix to rotate around the side vector
	float x=temp_side.x;
	float y=temp_side.y;
	float z=temp_side.z;
	float c=cos(TORADIANS(degrees));
	float s=sin(TORADIANS(degrees));
	float t=1.0 - cos(TORADIANS(degrees));
	float m00 = t*x*x + c;
	float m01 = t*x*y + s*z;
	float m02 = t*x*z - s*y;
	float m03 = 0;
	float m10 = t*x*y - s*z;
	float m11 = t*y*y + c;
	float m12 = t*y*z + s*x;
	float m13 = 0;
	float m20 = t*x*z + s*y;
	float m21 = t*y*z - s*x;
	float m22 = t*z*z + c;
	float m23 = 0;
	float m30 = 0;
	float m31 = 0;
	float m32 = 0;
	float m33 = 1;
	matrix4x4<float> rotation(m00, m01, m02, m03,
					   m10, m11, m12, m13,
					   m20, m21, m22, m23,
					   m30, m31, m32, m33);
	
	//rotate the up and look-at vectors around the side vector
	vector3D<float> result_lookat = rotation*temp_lookat;
	vector3D<float> result_up = rotation*temp_up;
	result_lookat.Normalize();
	result_up.Normalize();

	m_camera_state.lookat = m_camera_state.position + result_lookat * lookat_length;
	m_camera_state.up = result_up;
}
	

void rtsCamera::RotateLeftRight(double degrees)
//this function rotates the camera around the up vector (which always points along hte positive
//Y world axis).
{
	//translate the look-at point to the origin (and the camera with it)
	point3D<float> origin = point3D<float>(0.0, 0.0, 0.0);
	vector3D<float> translateCamera = origin-m_camera_state.lookat;

	point3D<float> translatedCamera=m_camera_state.position+translateCamera;


	//perform the rotation around the look-at point
	//using the y-axis as the rotation axis
	point3D<float> newcamera;
	newcamera.x=translatedCamera.x*cos(TORADIANS(degrees)) - translatedCamera.z*sin(TORADIANS(degrees));
	newcamera.z=translatedCamera.x*sin(TORADIANS(degrees)) + translatedCamera.z*cos(TORADIANS(degrees));
	newcamera.y=translatedCamera.y;

	vector3D<float> newup;
	newup.x=m_camera_state.up.x*cos(TORADIANS(degrees)) - m_camera_state.up.z*sin(TORADIANS(degrees));
	newup.z=m_camera_state.up.x*sin(TORADIANS(degrees)) + m_camera_state.up.z*cos(TORADIANS(degrees));
	newup.y=m_camera_state.up.y;

	//translate the lookat point back to it's original position (along with the camera)
	newcamera=newcamera-translateCamera;

    m_camera_state.position.x=newcamera.x;
	m_camera_state.position.y=newcamera.y;
	m_camera_state.position.z=newcamera.z;

	m_camera_state.up.x=newup.x;
	m_camera_state.up.y=newup.y;
	m_camera_state.up.z=newup.z;
	m_camera_state.up.Normalize();

}

void rtsCamera::Forward(double distance)
{
	//calculate the lookat vector (direction of travel)
	vector3D<float> old_lookat=m_camera_state.lookat-m_camera_state.position;
	old_lookat.Normalize();

	//calculate the new position of the camera
	point3D<float> new_position = m_camera_state.position+old_lookat*distance;
	//now calculate the new lookat vector
	vector3D<float> new_lookat=m_camera_state.lookat-new_position;
	//find the length of the new lookat vector
	/*double newlength=lookatvector.length();
	//if the length is 0 or the camera flipped
	if((newlength <= 0.0))
	{
		//recalculate the lookat vector using the old position
		lookatvector=m_camera_state.lookat-m_camera_state.position;
		lookatvector.Normalize();
		//adjust the lookat point appropriately
		m_camera_state.lookat = new_position + lookatvector;
	}*/
	//move the camera to the new position
	m_camera_state.position = new_position;
}

void rtsCamera::ScaleForward(double factor, double min, double max)
{
	/*This function moves the camera forward, scaling the magnitude
	of the motion by the length of the view vector.  Basically, the closer
	the camera is to the lookat point, the slower the camera moves.*/

	//calculate the lookat vector (direction of travel)
	vector3D<float> lookatvector=m_camera_state.lookat-m_camera_state.position;
	//find the length of the view vector
	double length = lookatvector.Length();
	//normalize
	lookatvector.Normalize();

	//prevent motion if the bounds would be passed
	double new_distance = length - (factor*length);
	if(new_distance < min || new_distance > max)
		factor = 0;
	//move the camera
	m_camera_state.position=m_camera_state.position+lookatvector*factor*length;
	lookatvector=m_camera_state.lookat-m_camera_state.position;
	/*double newlength=lookatvector.length();
	if((newlength < 2))
	{
		lookatvector.Normalize();
		m_camera_state.lookat = m_camera_state.position + lookatvector*2;
	}*/


}

void rtsCamera::DollyLeftRight(double distance)
{
	//calculate the side vector vector (direction of travel)
	vector3D<float> lookatvector=m_camera_state.lookat-m_camera_state.position;
	vector3D<float> side = lookatvector.X(m_camera_state.up);
	side.Normalize();	

	m_camera_state.position=m_camera_state.position+side*distance;
	m_camera_state.lookat = m_camera_state.lookat + side*distance;
	//lookatvector=m_camera_state.lookat-m_camera_state.position;
}

void rtsCamera::DollyUpDown(double distance)
{
	//move along the up vector
	m_camera_state.up.Normalize();	

	m_camera_state.position=m_camera_state.position+m_camera_state.up*distance;
	m_camera_state.lookat = m_camera_state.lookat + m_camera_state.up*distance;
	//lookatvector=m_camera_state.lookat-m_camera_state.position;
}

void rtsCamera::Zoom(double angle)
{
	m_camera_state.pers_view_angle += angle;
}