#include <../math/vector.h>
#include <../math/point.h>
#include <../math/quaternion.h>
#include <../math/matrix.h>

#include <ostream>

#ifndef STIM_CAMERA_H
#define STIM_CAMERA_H

namespace stim{

class camera
{
	vector<float, 3> d;	//direction that the camera is pointing
	point<float, 3> p;	//position of the camera
	vector<float, 3> 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()
	{
		vector<float, 3> side = up.cross(d);
		up = d.cross(side);
		up = up.norm();
		d = d.norm();
	}

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

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

	void LookAt(point<float, 3> pos)
	{
		//find the new direction
		d = pos - p;

		//find the distance from the look-at point to the current position
		focus = d.len();

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

	void Dolly(vector<float, 3> direction)
	{
		p = p+direction;
	}
	void Dolly(float x, float y, float z){Dolly(vector<float, 3>(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
		quaternion<float> qx;
		qx.CreateRotation(theta_x, up[0], up[1], up[2]);

		//y rotation is around the side axis
		vector<float, 3> side = up.cross(d);
		quaternion<float> qy;
		qy.CreateRotation(theta_y, side[0], side[1], side[2]);

		//z rotation is around the direction vector
		quaternion<float> qz;
		qz.CreateRotation(theta_z, d[0], d[1], d[2]);

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

		//get the rotation matrix
		matrix<float, 3> rot_matrix = final.toMatrix3();

		//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
		point<float, 3> focal_point = p + focus*d;

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

		//create the x rotation (around the up vector)
		quaternion<float> qx;
		qx.CreateRotation(theta_x, up[0], up[1], up[2]);
		centered = point<float, 3>(0, 0, 0) + qx.toMatrix3()*(centered - point<float, 3>(0, 0, 0));

		//get a side vector for theta_y rotation
		vector<float, 3> side = up.cross((point<float, 3>(0, 0, 0) - centered).norm());

		quaternion<float> qy;
		qy.CreateRotation(theta_y, side[0], side[1], side[2]);
		centered = point<float, 3>(0, 0, 0) + qy.toMatrix3()*(centered - point<float, 3>(0, 0, 0));

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

		//re-position the camera
		p = centered + (focal_point - point<float, 3>(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)
	{
		vector<float, 3> V = up.norm();
		vector<float, 3> U = up.cross(d).norm();

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

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

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

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

	//constructor
	camera()
	{
		p = point<float, 3>(0, 0, 0);
		d = vector<float, 3>(0, 0, 1);
		up = vector<float, 3>(0, 1, 0);
		focus = 1;

	}
};

}



#endif