#include "../cse452.h"
#include "Camera.h"
#include <cmath>
#include <FL/Fl.H>

Camera::Camera()
{
	k = nearClippingPlane / (double)farClippingPlane;
	theta_w = M_PI / 6.0; //atan(width / (2.0 * nearClippingPlane));
	theta_h = M_PI / 6.0; //atan(height / (2.0 * nearClippingPlane));

	updateD();
	updateSxyz();
	updateSxy();
	updateR();
	updateTp();
	updateT();
}

Camera::~Camera() {
    // destroy your data here
}

void Camera::updateD() {
	// Perspective transformation matrix
	D = Matrix4(
		Vector4(1, 0, 0, 0),
		Vector4(0, 1, 0, 0),
		Vector4(0, 0, 1 / (k - 1.0), k / (k - 1.0)),
		Vector4(0, 0, -1, 0)
		);
}

void Camera::updateSxyz() {
	// Transforms to box
	Sxyz = Matrix4(
		Vector4(1 / farClippingPlane, 0, 0, 0),
		Vector4(0, 1 / farClippingPlane, 0, 0),
		Vector4(0, 0, 1 / farClippingPlane, 0),
		Vector4(0, 0, 0, 1)
		);

	Sxyz_inv = Matrix4(
		Vector4(farClippingPlane, 0, 0, 0),
		Vector4(0, farClippingPlane, 0, 0),
		Vector4(0, 0, farClippingPlane, 0),
		Vector4(0, 0, 0, 1)
		);

	if (!((Sxyz * Sxyz_inv).approxEqual(Matrix4::identity()))) {
		std::cout << "Sxyz inverse is incorrect." << std::endl;
	}
}

void Camera::updateSxy() {
	// Scaling matrix for the x, y worldview to camera
	Sxy = Matrix4(
		Vector4(1 / tan(theta_w / 2.0), 0, 0, 0),
		Vector4(0, 1 / tan(theta_h / 2.0), 0, 0),
		Vector4(0, 0, 1, 0),
		Vector4(0, 0, 0, 1)
		);

	Sxy_inv = Matrix4(
		Vector4(tan(theta_w / 2.0), 0, 0, 0),
		Vector4(0, tan(theta_h / 2.0), 0, 0),
		Vector4(0, 0, 1, 0),
		Vector4(0, 0, 0, 1)
		);

	if (!((Sxy * Sxy_inv).approxEqual(Matrix4::identity()))) {
		std::cout << "Sxy inverse is incorrect." << std::endl;
	}
}

void Camera::updateR() {
	// Rotation
	R = Matrix4(
		Vector4(-u[0], -u[1], -u[2], 0),
		Vector4(v[0], v[1], v[2], 0),
		Vector4(-n[0],- n[1], -n[2], 0),
		Vector4(0, 0, 0, 1)
		);

	//R_inv = R.transpose();
	R_inv = Matrix4(
		Vector4(-u[0], v[0], -n[0], 0),
		Vector4(-u[1], v[1], -n[1], 0),
		Vector4(-u[2], v[2], -n[2], 0),
		Vector4(0, 0, 0, 1));

	if (!(R_inv == R.transpose())) {
		std::cout << "R inverse is incorrect." << std::endl;
	}
}

void Camera::updateTp() {
	// Transformation matrix for position of camera
	Tp = Matrix4(
		Vector4(0, 0, 0, eye[0]),
		Vector4(0, 0, 0, eye[1]),
		Vector4(0, 0, 0, eye[2]),
		Vector4(0, 0, 0, 1)
		);
}

void Camera::updateT() {
	// Translation matrix
	T = Matrix4(
		Vector4(1, 0, 0, -eye[0]),
		Vector4(0, 1, 0, -eye[1]),
		Vector4(0, 0, 1, -eye[2]),
		Vector4(0, 0, 0, 1)
		);

	T_inv = Matrix4(
		Vector4(1, 0, 0, eye[0]),
		Vector4(0, 1, 0, eye[1]),
		Vector4(0, 0, 1, eye[2]),
		Vector4(0, 0, 0, 1)
		);

	if (!((T * T_inv).approxEqual(Matrix4::identity()))) {
		std::cout << "T inverse is incorrect." << std::endl;
	}
}

// The following three should be unit length and orthogonal to each other
// u vector
Vector3 Camera::getRight() const
{
    return u;
}

// v vector
Vector3 Camera::getUp() const
{
    return v;
}

// - n vector
Vector3 Camera::getLook() const
{
	return -n;
}

double Camera::getSkew() const
{
    // Change this to implement the extra credit
    return skew;
}

double Camera::getAspectRatioScale() const
{
    // Change this to implement the extra credit
	return aspectRatioScale;
}

Point3 Camera::getProjectionCenter() const
{
    // Change this to implement the extra credit
	return projCenter;
}

Matrix4 Camera::getProjection() const
{
    // return the current projection and scale matrix
	return D*Sxyz;
}

Matrix4 Camera::getWorldToCamera() const
{
 	return R*T;
}

Matrix4 Camera::getRotationFromXYZ() const
{
    // return just the rotation matrix
	return R; // world -> cam
}

Matrix4 Camera::getRotationToXYZ() const
{
    // return just the rotation matrix
    return R_inv; // cam -> world
}

Matrix4 Camera::getCameraToWorld() const
{
    // return the current camera to world matrix
    // This is the inverse of the rotation, translation, and scale
	return Sxyz_inv * Sxy_inv * R_inv * T_inv;
}

int Camera::getWidth() const
{
    // return the current image width
	return width;
}

int Camera::getHeight() const
{
    // return the current image height
	return height;
}

Point3 Camera::getEye() const
{
    // return the current eye location
    return eye;
}

double Camera::getZoom() const
{
	return theta_h;
}

void Camera::setFrom(const Point3& from)
{
    // set the current viewpoint (eye point)
	eye = from;
	updateT();
}

void Camera::setAt(const Point3& at)
{
    // set the point the camera is looking at
    // calling this requires that the from (or eye) point already be valid
	look = eye - at;
	n = -look;
	n.normalize();

	v = v - ((v * -n) / (-n * -n))*-n;

	u = cross(v, n);
	u.normalize();
	updateR();
}

void Camera::setLook(const Vector3& l)
{
    // set the direction the camera is looking at
	look = l;
	look.normalize();
	n = -look;
	v = v - ((v*look) / (look*look))*look;
	u = v^n;
	u.normalize();
	v.normalize();
	n.normalize();
	updateR();
}

void Camera::setUp(const Vector3& up)
{
    // set the upwards direction of the camera
	v = up;
	v.normalize();
	look = look - ((look*v) / (v*v)) *v;
	n = -look;
	u = v^n;
	u.normalize();
	v.normalize();
	n.normalize();
	updateR();
}

void Camera::setWidthHeight(int w, int h)
{
    // set the current width and height of the film plane
	width = w;
	height = h;
	theta_w = 2.0 * atan(width / (2.0 * nearClippingPlane));
	theta_h = 2.0 * atan(height / (2.0 * nearClippingPlane));

	updateSxy();
}

void Camera::setZoom(double z)
{
    // set field of view (specified in degrees)
	theta_h = (2.0*M_PI*z)/360;
	theta_w = 2.0*atan(tan(theta_h / 2.0)*(width / height));
	updateSxy();
}

void Camera::setNearFar(double n, double f)
{
    // set the near and far clipping planes
	nearClippingPlane = n;
	farClippingPlane = f;
	k = n / f;
	theta_w = 2.0 * atan(width / (2.0 * nearClippingPlane));
	theta_h = 2.0 * atan(height / (2.0 * nearClippingPlane));
	updateSxyz();
	updateD();
	updateSxy();
}

void Camera::setSkew( double d )
{
	skew = d;
}

void Camera::setAspectRatioScale( double d )
{
	aspectRatioScale = d;
}

void Camera::setProjectionCenter( const Point3 &p )
{
	projCenter = p;
}

void Camera::moveForward(double dist) {
    // move the camera forward (in the viewing direction)
    // by the amount dist
	eye = eye + n*dist;
	updateTp();
	updateT();
}

void Camera::moveSideways(double dist) {
    // move the camera sideways (orthogonal to the viewing direction)
    // by the amount dist
	eye = eye + -getRight()*dist;
	updateTp();
	updateT();
}

void Camera::moveVertical(double dist) {
    // move the camera vertically (along the up vector)
    // by the amount dist
	eye = eye + getUp()*dist;
	updateTp();
	updateT();
}

void Camera::rotateYaw(double angle) {
    // rotate the camera left/right (around the up vector)
	look = look*cos(angle) - getRight()*sin(angle);
	look.normalize();
	n = -look;
	n.normalize();
	u = v^n;
	u.normalize();
	updateTp();
	updateT();
	updateR();
}

void Camera::rotatePitch(double angle) {
    // rotate the camera up/down (pitch angle)
	look = look*cos(angle) - getUp()*sin(angle);
	look.normalize();
	n = -look;
	n.normalize();
	u = v^n;
	u.normalize();
	updateTp();
	updateT();
	updateR();
}

void Camera::rotateAroundAtPoint(int axis, double angle, double focusDist)
{

}


void Camera::moveKeyboard( )
{
    // you may change key controls for the interactive
    // camera controls here, make sure you document your changes
    // in your README file

    if (Fl::event_key('w'))
        moveForward(+0.05);
    if (Fl::event_key('s'))
        moveForward(-0.05);
    if (Fl::event_key('a'))
        moveSideways(-0.05);
    if (Fl::event_key('d'))
        moveSideways(+0.05);
    if (Fl::event_key(FL_Up))
        moveVertical(+0.05);
    if (Fl::event_key(FL_Down))
        moveVertical(-0.05);
    if (Fl::event_key(FL_Left))
        rotateYaw(+0.05);
    if (Fl::event_key(FL_Right))
        rotateYaw(-0.05);
    if (Fl::event_key(FL_Page_Up))
        rotatePitch(+0.05);
    if (Fl::event_key(FL_Page_Down))
        rotatePitch(-0.05);
}