#include <Camera.hpp>

Camera::Camera()
 : fwd(Eigen::Vector3d::UnitX()), up(Eigen::Vector3d::UnitZ()), R(Eigen::Matrix3d::Identity()), R_T(Eigen::Matrix3d::Identity()), FOV(90.0*DEG2RAD), tanFOV2(tan(FOV/2.0))
{
    BuildRotationMatrix();
}

// Getters
const Eigen::Vector3d& Camera::GetForward() const {
    return fwd;
}

const Eigen::Vector3d& Camera::GetUp() const {
    return up;
}

const Eigen::Matrix3d& Camera::GetR() const {
    return R;
}

const Eigen::Matrix3d& Camera::GetR_T() const {
    return R_T;
}

double Camera::GetFOV() const {
    return FOV;
}

// Setters
Camera& Camera::SetForward(const Eigen::Vector3d& fwd_) {
    fwd = fwd_;
    return *this;
}

Camera& Camera::SetUp(const Eigen::Vector3d& up_) {
    up = up_;
    return *this;
}

Camera& Camera::SetR(const Eigen::Matrix3d& R_) {
    R = R_;
    R_T = R.transpose();
    return *this;
}

Camera& Camera::SetFOV(double fov) {
    FOV = fov;
    tanFOV2 = tan(FOV/2.0);
    return *this;
}

Camera & Camera::BuildRotationMatrix() {
    Eigen::Vector3d left   = up.cross(fwd);
    left                   = left/left.norm();
    Eigen::Vector3d cam_up = fwd.cross(left);
    R.col(0)               = -left;        // x-axis is to the right
    R.col(1)               = cam_up;       // y-axis is up
    R.col(2)               = -fwd;         // z-axis is backwards
    R_T = R.transpose();
    return *this;
}

Camera & Camera::SetTarget(Eigen::Vector3d const& target) {
    fwd = target/target.norm();
    BuildRotationMatrix();
    return *this;
}

Camera & Camera::SetEulerAngles(double yaw, double pitch, double roll, bool radians) {
    if(!radians) {
        yaw   *= DEG2RAD;
        pitch *= DEG2RAD;
        roll  *= DEG2RAD;
    }

    // Calculate sin and cos values
    const double s_roll = std::sin(roll);
    const double c_roll = std::cos(roll);
    const double s_pitch = std::sin(pitch);
    const double c_pitch = std::cos(pitch);
    const double s_yaw = std::sin(yaw);
    const double c_yaw = std::cos(yaw);

    // Build rotation matrix
    R << c_pitch * c_yaw, -c_pitch * s_yaw, s_pitch,
         s_roll * s_pitch * c_yaw + c_roll * s_yaw, -s_roll * s_pitch * s_yaw + c_roll * c_yaw, -s_roll * c_pitch,
         -c_roll * s_pitch * c_yaw + s_roll * s_yaw, c_roll * s_pitch * s_yaw + s_roll * c_yaw, c_roll * c_pitch;
    
    R_T = R.transpose();
    return *this;
}

Eigen::Vector3d Camera::ComputeRayDirInCameraFrame(Eigen::Vector2d const& p_norm) const {
    return Eigen::Vector3d(p_norm(0)*tanFOV2, p_norm(1)*tanFOV2, -1.0).normalized();
}

Eigen::Vector3d Camera::ComputeRayDirInInertialFrame(Eigen::Vector2d const& p_norm) const {
    return R*ComputeRayDirInCameraFrame(p_norm);
}

Eigen::Vector3d Camera::ProjectPointToSensorCameraFrame(Eigen::Vector3d const& p_camera) const {
    Eigen::Vector3d p_sensor = p_camera;
    if(p_camera[2] != 0.0)
        p_sensor /= tanFOV2*std::abs(p_camera[2]);
    return p_sensor;
}

Eigen::Vector3d Camera::ProjectPointToSensorInertialFrame(Eigen::Vector3d const& p_inertial) const {
    return ProjectPointToSensorCameraFrame(R_T*p_inertial);
}

bool Camera::IsPointVisibleCameraFrame(Eigen::Vector3d const& p_camera) const {
    Eigen::Vector3d p_sensor = ProjectPointToSensorCameraFrame(p_camera);
    return (p_sensor[0] >= -1.0) && (p_sensor[0] <= 1.0) && (p_sensor[1] >= -1.0) && (p_sensor[1] <= 1.0) && (p_sensor[2] < 0.0);
}

bool Camera::IsPointVisibleInertialFrame(Eigen::Vector3d const& p_inertial) const {
    Eigen::Vector3d p_sensor = ProjectPointToSensorInertialFrame(p_inertial);
    return (p_sensor[0] >= -1.0) && (p_sensor[0] <= 1.0) && (p_sensor[1] >= -1.0) && (p_sensor[1] <= 1.0) && (p_sensor[2] < 0.0);
}

Eigen::Vector2d Camera::PixelToNormalizedCoordinates(unsigned int i, unsigned int j, unsigned int width, unsigned int height) {
    return Eigen::Vector2d(2.0*((double)j)/(width - 1) - 1.0, -2.0*((double)i)/(height - 1) + 1.0);
}