#include "rtsCameraController.h"
#include <math.h>
rtsCamera::rtsCamera()
{
	position = point3D<float>(0, 0, 0);
	view_vector = vector3D<float>(0, 0, -1);
	up_vector = vector3D<float>(0, 1, 0);
	lookat_point = point3D<float>(0, 0, -1);

	pers_view_angle = 60;
	ortho_width = 1.0;
	ortho_height = 1.0;

	near_plane = 1;
	far_plane = 100;
}

rtsCamera::LookAt(

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




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

	//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;
	
}

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;
}

*/