#ifndef H_Camera
#define H_Camera

#include <Eigen/Dense>

#define DEG2RAD 0.01745329251994329576923690768488612713442871888541725456097191440171009114603449443682241569634509482
#define RAD2DEG 57.29577951308232087679815481410517033240547246656432154916024386120284714832155263244096899585111094

/// @brief A camera class to perform frame transformations and projections.
class Camera {
    public:
        // Constructors
        Camera();

        // Accessors
        Eigen::Vector3d const& GetForward() const;
        Eigen::Vector3d const& GetUp() const;
        Eigen::Matrix3d const& GetR() const;
        Eigen::Matrix3d const& GetR_T() const;
        double GetFOV() const;

        // Setters following the Builder pattern. See https://en.wikipedia.org/wiki/Builder_pattern
        Camera & SetForward(Eigen::Vector3d const& fwd_);
        Camera & SetUp(Eigen::Vector3d const& up_);
        Camera & SetR(Eigen::Matrix3d const& R_);
        Camera & SetFOV(double fov);

        // Methods
        /// @brief Build the rotation matrix from the camera frame to the inertial frame from the fwd and up vectors.
        Camera & BuildRotationMatrix();

        /// @brief Orients the camera so that the forward vector points at the target. The position of the camera is (0,0,0).
        Camera & SetTarget(Eigen::Vector3d const& target);

        /// @brief Orients the camera according to the provided Euler angles. The position of the camera is (0,0,0).
        Camera & SetEulerAngles(double yaw, double pitch, double roll, bool radians = false);

        /// @brief Computes the ray direction in camera frame for a point (x,y) on the surface of the camera's sensor. p_norm is in normalized coordinates [-1;1].
        /// @param p_norm Normalized coordinates of the point on the sensor.
        /// @return The ray direction in camera frame, cartesian coordinates.
        Eigen::Vector3d ComputeRayDirInCameraFrame(Eigen::Vector2d const& p_norm) const;

        /// @brief Computes the ray direction in inertial frame for a point (x,y) on the surface of the camera's sensor. p_norm is in normalized coordinates [-1;1].
        /// @param p_norm Normalized coordinates of the point on the sensor.
        /// @return The ray direction in inertial frame, cartesian coordinates.
        Eigen::Vector3d ComputeRayDirInInertialFrame(Eigen::Vector2d const& p_norm) const;

        /// @brief Projects a point in camera frame (cartesian coordinates) to the sensor.
        /// @param p_inertial Cartesian coordinates of the point to project, in camera frame.
        /// @return Sensor coordinates of the projected point. The Z component is +/- tan(FOV/2). Negative Z means the point is in front of the camera.
        Eigen::Vector3d ProjectPointToSensorCameraFrame(Eigen::Vector3d const& p_camera) const;

        /// @brief Projects a point in inertial frame (cartesian coordinates) to the sensor.
        /// @param p_inertial Cartesian coordinates of the point to project, in inertial frame.
        /// @return Sensor coordinates of the projected point. The Z component is +/- tan(FOV/2). Negative Z means the point is in front of the camera.
        Eigen::Vector3d ProjectPointToSensorInertialFrame(Eigen::Vector3d const& p_inertial) const;

        /// @brief Checks if a point in camera frame is visible by the camera.
        /// @param p_inertial Cartesian coordinates of the point to check, in camera frame.
        /// @return True if the point is visible, false otherwise.
        bool IsPointVisibleCameraFrame(Eigen::Vector3d const& p_camera) const;

        /// @brief Checks if a point in inertial frame is visible by the camera.
        /// @param p_inertial Cartesian coordinates of the point to check, in inertial frame.
        /// @return True if the point is visible, false otherwise.
        bool IsPointVisibleInertialFrame(Eigen::Vector3d const& p_inertial) const;

        // Static methods
        /// @brief Converts pixel coordinates to normalized coordinates. i is the row (y), j is the column (x). Normalized coordinates range from -1 to 1.
        static Eigen::Vector2d PixelToNormalizedCoordinates(unsigned int i, unsigned int j, unsigned int width, unsigned int height);
    
    private:
        Eigen::Vector3d fwd;    //!< Forward vector in inertial frame
        Eigen::Vector3d up;     //!< Up vector in inertial frame
        Eigen::Matrix3d R;      //!< Rotation matrix from camera frame to inertial frame
        Eigen::Matrix3d R_T;    //!< Rotation matrix from inertial frame to camera frame
        double FOV;             //!< Field of view in radians
        double tanFOV2;         //!< tan(FOV/2)
};

#endif