#include "rtsVector3d.h"
#include "rtsPoint3d.h"
#include "rtsQuaternion.h"

#ifndef RTS_CAMERA_H
#define RTS_CAMERA_H

class rtsCamera
{
	vector3D<float> d;	//direction that the camera is pointing
	point3D<float> p;	//position of the camera
	vector3D<float> up;	//"up" direction
	float focus;		//focal length of the camera
	float fov;

	//private function makes sure that the up vector is orthogonal to the direction vector and both are normalized
	void stabalize()
	{
		vector3D<float> side = up.X(d);
		up = d.X(side);
		up.Normalize();
		d.Normalize();
	}

public:
	void setPosition(point3D<float> pos)
	{
		p = pos;
	}
	void setPosition(float x, float y, float z){setPosition(point3D<float>(x, y, z));}

	void setFocalDistance(float distance){focus = distance;}
	void setFOV(float field_of_view){fov = field_of_view;}

	void LookAt(point3D<float> pos)
	{
		//find the new direction
		d = pos - p;
	
		//find the distance from the look-at point to the current position
		focus = d.Length();

		//stabalize the camera
		stabalize();
	}
	void LookAt(float px, float py, float pz){LookAt(point3D<float>(px, py, pz));}
	void LookAt(point3D<float> pos, vector3D<float> new_up){up = new_up; LookAt(pos);}
	void LookAt(float px, float py, float pz, float ux, float uy, float uz){LookAt(point3D<float>(px, py, pz), vector3D<float>(ux, uy, uz));}
	void LookAtDolly(float lx, float ly, float lz)
	{
		//find the current focus point
		point3D<float> f = p + focus*d;
		vector3D<float> T = point3D<float>(lx, ly, lz) - f;
		p = p + T;
	}

	void Dolly(vector3D<float> direction)
	{
		p = p+direction;
	}
	void Dolly(float x, float y, float z){Dolly(vector3D<float>(x, y, z));}
	void Push(float delta)
	{
		if(delta > focus)
			delta = focus;

		focus -= delta;

		Dolly(d*delta);
	}

	void Pan(float theta_x, float theta_y, float theta_z)
	{
		//x rotation is around the up axis
		rtsQuaternion<float> qx;
		qx.CreateRotation(theta_x, up.x, up.y, up.z);

		//y rotation is around the side axis
		vector3D<float> side = up.X(d);
		rtsQuaternion<float> qy;
		qy.CreateRotation(theta_y, side.x, side.y, side.z);

		//z rotation is around the direction vector
		rtsQuaternion<float> qz;
		qz.CreateRotation(theta_z, d.x, d.y, d.z);

		//combine the rotations in x, y, z order
		rtsQuaternion<float> final = qz*qy*qx;

		//get the rotation matrix
		matrix4x4<float> rot_matrix = final.toMatrix();

		//apply the rotation
		d = rot_matrix*d;
		up = rot_matrix*up;

		//stabalize the camera
		stabalize();

	}
	void Pan(float theta_x){Pan(theta_x, 0, 0);}
	void Tilt(float theta_y){Pan(0, theta_y, 0);}
	void Twist(float theta_z){Pan(0, 0, theta_z);}

	void Zoom(float delta)
	{
		fov -= delta;
		if(fov < 0.5)
			fov = 0.5;
		if(fov > 180)
			fov = 180;
	}

	void OrbitFocus(float theta_x, float theta_y)
	{
		//find the focal point
		point3D<float> focal_point = p + focus*d;

		//center the coordinate system on the focal point
		point3D<float> centered = p - (focal_point - point3D<float>(0, 0, 0));

		//create the x rotation (around the up vector)
		rtsQuaternion<float> qx;
		qx.CreateRotation(theta_x, up.x, up.y, up.z);
		centered = qx.toMatrix()*centered;

		//get a side vector for theta_y rotation
		vector3D<float> side = up.X((point3D<float>(0, 0, 0) - centered).Normalize());

		rtsQuaternion<float> qy;
		qy.CreateRotation(theta_y, side.x, side.y, side.z);
		centered = qy.toMatrix()*centered;

		//perform the rotation on the centered camera position
		//centered = final.toMatrix()*centered;

		//re-position the camera
		p = centered + (focal_point - point3D<float>(0, 0, 0));

		//make sure we are looking at the focal point
		LookAt(focal_point);

		//stabalize the camera
		stabalize();	

	}

	void Slide(float u, float v)
	{
		vector3D<float> V = up.Normalize();
		vector3D<float> U = up.X(d).Normalize();

		p = p + (V * v) + (U * u);
	}

	//accessor methods
	point3D<float> getPosition(){return p;}
	vector3D<float> getUp(){return up;}
	vector3D<float> getDirection(){return d;}
	point3D<float> getLookAt(){return p + focus*d;}
	float getFOV(){return fov;}

	//output the camera settings
	const void print(ostream& output)
	{
		output<<"Position: "<<p<<endl;

	}
	friend ostream& operator<<(ostream& out, const rtsCamera& c)
	{
		out<<"Position: "<<c.p<<endl;
		out<<"Direction: "<<c.d<<endl;
		out<<"Up: "<<c.up<<endl;
		out<<"Focal Distance: "<<c.focus<<endl;
		return out;
	}

	//constructor
	rtsCamera()
	{
		p = point3D<float>(0, 0, 0);
		d = vector3D<float>(0, 0, 1);
		up = vector3D<float>(0, 1, 0);
		focus = 1;

	}
};



#endif