//
// Created by trotfunky on 19/10/2019.
//

#include "Camera.h"


Camera::Camera(const Vec3d& eye_pos, const Vec3d& look_direction, const Vec3d& up_vector) :
    eye(eye_pos),
    gaze(look_direction),
    gaze_up(up_vector)
{
    // Normalize the gaze vectors as they form the base for the local coordinates
    gaze.normalize();
    gaze_up.normalize();

    compute_base_change();
}

void Camera::translate(const Vec3d& translation)
{
    eye = eye + translation * rotation_quaternion;
}

void Camera::rotate(const Vec3d& rotation)
{
    Quaternion x_rotation{gaze*sin(rotation.x/2)};
    x_rotation.w = cos(rotation.x/2);
    x_rotation.normalize();
    gaze_up = x_rotation * gaze_up;

    Quaternion z_rotation{gaze.cross_product(gaze_up)*sin(rotation.z/2)};
    z_rotation.w = cos(rotation.z/2);
    z_rotation.normalize();
    gaze = z_rotation * gaze;
    gaze_up = z_rotation * gaze_up;

    Quaternion y_rotation{0,sin(rotation.y),0,cos(rotation.y)};
    y_rotation.normalize();
    gaze = y_rotation * gaze;
    gaze_up = y_rotation * gaze_up;

    compute_base_change();
}

void Camera::look()
{
    gluLookAt(eye.x,eye.y,eye.z,
            eye.x+gaze.x,eye.y+gaze.y,eye.z+gaze.z,
            gaze_up.x,gaze_up.y,gaze_up.z);
}

void Camera::set_position(const Vec3d& eye_pos)
{
    gaze = eye_pos;
}

void Camera::compute_base_change()
{
    // Third vector of the base, should already be normalized
    Vec3d gaze_normal = gaze.cross_product(gaze_up);

    double quaternion_w = std::sqrt(1 + gaze.x + gaze_up.y + gaze_normal.z) / 2;

    rotation_quaternion = {(gaze_normal.y - gaze_up.z)/(4*quaternion_w),
                           (gaze.z - gaze_normal.x)/(4*quaternion_w),
                           (gaze_up.x - gaze.y)/(4*quaternion_w),
                           quaternion_w};
    rotation_quaternion.normalize();
}