#pragma once
#include <cmath>
#include <math.h>

namespace SimCore {

    class EulerConversions{
    public: 
    
    /**
     * Gets a degree heading for an entity based on euler angles. All angular
     * values passed in must be in radians.
     *
     * @param lat Entity's latitude, IN RADIANS
     * @param lon Entity's longitude, IN RADIANS
     * @param psi Psi angle, IN RADIANS
     * @param theta Theta angle, IN RADIANS
     * @return the heading, in degrees, with 0 being north, positive angles
     * going clockwise, and negative angles going counterclockwise (i.e., 90 deg
     * is east, -90 is west)
     */
    static double getOrientationFromEuler(double lat, double lon, double psi, double theta);

    /**
     * Gets a degree pitch for an entity based on euler angles. All angular
     * values passed in must be in radians.
     *
     * @param lat Entity's latitude, IN RADIANS
     * @param lon Entity's longitude, IN RADIANS
     * @param psi Psi angle, IN RADIANS
     * @param theta Theta angle, IN RADIANS
     * @return the pitch, in degrees, with 0 being level. A negative values is
     * when the entity's nose is pointing downward, positive value is when the
     * entity's nose is pointing upward.
     */
     static double getPitchFromEuler(double lat, double lon, double psi, double theta);

/**
     * Gets the degree roll for an entity based on euler angles. All angular
     * values passed in must be in radians.
     *
     * @param lat Entity's latitude, IN RADIANS
     * @param lon Entity's longitude, IN RADIANS
     * @param psi Psi angle, IN RADIANS
     * @param theta Theta angle, IN RADIANS
     * @param phi Phi angle, IN RADIANS
     * @return the roll, in degrees, with 0 being level flight, + roll is
     * clockwise when looking out the front of the entity.
     */
     static double getRollFromEuler(double lat, double lon, double psi, double theta, double phi);

    /**
     * Gets the Euler Theta value (in radians) from position and Tait-Brayn yaw
     * and pitch angles
     *
     * @param lat Entity's latitude, IN RADIANS
     * @param lon Entity's longitude, IN RADIANS
     * @param yaw entity's yaw angle (also know as the entity's bearing or
     * heading angle), in degrees
     * @param pitch entity's pitch angle, in degrees
     * @return the Theta value in radians
     */
     static double getThetaFromTaitBryanAngles(double lat, double lon, double yaw, double pitch);

    /**
     * Gets the Euler Psi value (in radians) from position and Tait-Brayn yaw
     * and pitch angles
     *
     * @param lat Entity's latitude, IN RADIANS
     * @param lon Entity's longitude, IN RADIANS
     * @param yaw ettity's yaw angle (also know as the entity's bearing or
     * heading angle), in degrees
     * @param pitch entity's pitch angle, in degrees
     * @return the Psi value in radians
     */
     static double getPsiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch);

    /**
     * Gets the Euler Phi value (in radians) from position and Tait-Brayn yaw,
     * pitch and roll angles
     *
     * @param lat Entity's latitude, IN RADIANS
     * @param lon Entity's longitude, IN RADIANS
     * @param yaw yaw angle (also know as the entity's bearing or heading
     * angle), in degrees
     * @param pitch entity's pitch angle, in degrees
     * @param roll entity's roll angle (0 is level flight, + roll is clockwise
     * looking out the nose), in degrees
     * @return the Phi value in radians
     */
     static double getPhiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch, double roll);

    
    private:

    static double toDegrees(double radian);

    constexpr static const  double _toDegrees = 57.2957795131;
    constexpr static double _toRadians = 0.01745329252;

};
}    

