#include <Camera.hpp>
#include <iostream>

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;
    }

    // Compute sin and cos values for the angles
    const double sy = sin(yaw);
    const double cy = cos(yaw);
    const double sp = sin(pitch);
    const double cp = cos(pitch);
    const double sr = sin(roll);
    const double cr = cos(roll);

    // Compute the rotation matrix
    R(0,0) = cy*cp;
    R(0,1) = cy*sp*sr - sy*cr;
    R(0,2) = cy*sp*cr + sy*sr;
    R(1,0) = sy*cp;
    R(1,1) = sy*sp*sr + cy*cr;
    R(1,2) = sy*sp*cr - cy*sr;
    R(2,0) = -sp;
    R(2,1) = cp*sr;
    R(2,2) = cp*cr;

    // Align the axes so that -Z points forward, Y points up, and X points to the right
    Eigen::Matrix3d R_Cam_Alignment; R_Cam_Alignment <<  0.,  0., -1.,
                                                        -1.,  0.,  0.,
                                                         0.,  1.,  0;
    R = R*R_Cam_Alignment;

    R_T = R.transpose();
    return *this;
}

Eigen::Vector3d Camera::GetEulerAngles() const
{
    // Align the axes so that -Z points forward, Y points up, and X points to the right
    Eigen::Matrix3d R_Cam_Alignment; R_Cam_Alignment <<  0.,  0., -1.,
                                                        -1.,  0.,  0.,
                                                         0.,  1.,  0;
    Eigen::Matrix3d _R = R*R_Cam_Alignment.transpose();

    double sy = sqrt(_R(0,0)*_R(0,0) + _R(1,0)*_R(1,0));

    double yaw, pitch, roll;
    if (sy > 1e-6) {
        yaw   = atan2(_R(1,0), _R(0,0));
        pitch = atan2(-_R(2,0), sy);
        roll  = atan2(_R(2,1), _R(2,2));
    }
    else {
        yaw   = atan2(-_R(0,1), _R(1,1));
        pitch = atan2(-_R(2,0), sy);
        roll  = 0.0;
    }

    return Eigen::Vector3d(yaw*RAD2DEG, pitch*RAD2DEG, roll*RAD2DEG);
}

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);
}

Camera Camera::FromYawPitchRollFOV(std::vector<double> const& yaw_pitch_roll_fov) {
    if(yaw_pitch_roll_fov.size() != 4) {
        std::cerr << "Error: Camera::FromYawPitchRollFOV: yaw_pitch_roll_fov must be a vector of size 4." << std::endl;
        return Camera();
    }
    return Camera().SetEulerAngles(yaw_pitch_roll_fov[0], yaw_pitch_roll_fov[1], yaw_pitch_roll_fov[2]).SetFOV(yaw_pitch_roll_fov[3]*DEG2RAD);
}