#include <stim/math/vector.h>
#include <stim/math/quaternion.h>
#include <stim/math/matrix_sq.h>

#include <ostream>
#include <iostream>

#ifndef STIM_CAMERA_H
#define STIM_CAMERA_H

namespace stim{

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

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

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

	void LookAt(vec3<float> 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(vec3<float>(px, py, pz));}
	void LookAt(vec3<float> pos, vec3<float> new_up){up = new_up; LookAt(pos);}
	void LookAt(float px, float py, float pz, float ux, float uy, float uz){LookAt(vec3<float>(px, py, pz), vec3<float>(ux, uy, uz));}
	void LookAtDolly(float lx, float ly, float lz)
	{
		//find the current focus point
		vec3<float> f = p + focus*d;
		vec3<float> T = vec3<float>(lx, ly, lz) - f;
		p = p + T;
	}

	void Dolly(vec3<float> direction)
	{
		p = p+direction;
	}
	void Dolly(float x, float y, float z){Dolly(vec3<float>(x, y, z));}

	/// Push the camera along the view direction
	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
		vec3<float> 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_sq<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
		vec3<float> focal_point = p + focus*d;

		//center the coordinate system on the focal point
		vec3<float> centered = p - (focal_point - vec3<float>(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 = vec3<float>(0, 0, 0) + qx.toMatrix3()*(centered - vec3<float>(0, 0, 0));

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

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

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

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

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

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

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

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

	//constructor
	camera()
	{
		p = vec3<float>(0, 0, 0);
		d = vec3<float>(0, 0, 1);
		up = vec3<float>(0, 1, 0);
		focus = 1;
		fov = 60;

	}

	/// Outputs the camera information as a string
	std::string str(){
		std::stringstream ss;
		ss<<p.str()<<"----->"<<(p + d * focus).str();
		return ss.str();
	}
};

}



#endif