#include <SimCore/EulerConversion.hpp>





namespace SimCore 
{
 double EulerConversions::getOrientationFromEuler(double lat, double lon, double psi, double theta) {
        double sinlat = sin(lat);
        double sinlon = sin(lon);
        double coslon = cos(lon);
        double coslat = cos(lat);
        double sinsin = sinlat * sinlon;

        double cosTheta = cos(theta);
        double cosPsi = cos(psi);
        double sinPsi = sin(psi);
        double sinTheta = sin(theta);

        double cosThetaCosPsi = cosTheta * cosPsi;
        double cosThetaSinPsi = cosTheta * sinPsi;
        double sincos = sinlat * coslon;

        double b11 = -sinlon * cosThetaCosPsi + coslon * cosThetaSinPsi;
        double b12 = -sincos * cosThetaCosPsi - sinsin * cosThetaSinPsi - coslat * sinTheta;

        return toDegrees(atan2(b11, b12));//range is -pi to pi
    };



    double EulerConversions::getPitchFromEuler(double lat, double lon, double psi, double theta) {
        double sinlat = sin(lat);
        double sinlon = sin(lon);
        double coslon = cos(lon);
        double coslat = cos(lat);
        double cosLatCosLon = coslat * coslon;
        double cosLatSinLon = coslat * sinlon;

        double cosTheta = cos(theta);
        double cosPsi = cos(psi);
        double sinPsi = sin(psi);
        double sinTheta = sin(theta);

        return toDegrees(asin(cosLatCosLon * cosTheta * cosPsi + cosLatSinLon * cosTheta * sinPsi - sinlat * sinTheta));
    };



    double EulerConversions::getRollFromEuler(double lat, double lon, double psi, double theta, double phi) {
        double sinlat = sin(lat);
        double sinlon = sin(lon);
        double coslon = cos(lon);
        double coslat = cos(lat);
        double cosLatCosLon = coslat * coslon;
        double cosLatSinLon = coslat * sinlon;

        double cosTheta = cos(theta);
        double sinTheta = sin(theta);
        double cosPsi = cos(psi);
        double sinPsi = sin(psi);
        double sinPhi = sin(phi);
        double cosPhi = cos(phi);

        double sinPhiSinTheta = sinPhi * sinTheta;
        double cosPhiSinTheta = cosPhi * sinTheta;

        double b23 = cosLatCosLon * (-cosPhi * sinPsi + sinPhiSinTheta * cosPsi)
                + cosLatSinLon * (cosPhi * cosPsi + sinPhiSinTheta * sinPsi)
                + sinlat * (sinPhi * cosTheta);

        double b33 = cosLatCosLon * (sinPhi * sinPsi + cosPhiSinTheta * cosPsi)
                + cosLatSinLon * (-sinPhi * cosPsi + cosPhiSinTheta * sinPsi)
                + sinlat * (cosPhi * cosTheta);

        return toDegrees(atan2(-b23, -b33));
    };


    double EulerConversions::getThetaFromTaitBryanAngles(double lat, double lon, double yaw, double pitch) {
        double sinLat = sin(lat);
        double cosLat = cos(lat);

        double cosPitch = cos(pitch * _toRadians);
        double sinPitch = sin(pitch * _toRadians);
        double cosYaw = cos(yaw * _toRadians);

        return asin(-cosLat * cosYaw * cosPitch - sinLat * sinPitch);
    };



    double EulerConversions::getPsiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch) {

        double sinLat = sin(lat);
        double sinLon = sin(lon);
        double cosLon = cos(lon);
        double cosLat = cos(lat);
        double cosLatCosLon = cosLat * cosLon;
        double cosLatSinLon = cosLat * sinLon;
        double sinLatCosLon = sinLat * cosLon;
        double sinLatSinLon = sinLat * sinLon;

        double cosPitch = cos(pitch * _toRadians);
        double sinPitch = sin(pitch * _toRadians);
        double sinYaw = sin(yaw * _toRadians);
        double cosYaw = cos(yaw * _toRadians);

        double a_11 = -sinLon * sinYaw * cosPitch - sinLatCosLon * cosYaw * cosPitch + cosLatCosLon * sinPitch;
        double a_12 = cosLon * sinYaw * cosPitch - sinLatSinLon * cosYaw * cosPitch + cosLatSinLon * sinPitch;

        return atan2(a_12, a_11);
    };



    double EulerConversions::getPhiFromTaitBryanAngles(double lat, double lon, double yaw, double pitch, double roll) {

        double sinLat = sin(lat);
        double cosLat = cos(lat);

        double cosRoll = cos(roll * _toRadians);
        double sinRoll = sin(roll * _toRadians);
        double cosPitch = cos(pitch * _toRadians);
        double sinPitch = sin(pitch * _toRadians);
        double sinYaw = sin(yaw * _toRadians);
        double cosYaw = cos(yaw * _toRadians);

        double a_23 = cosLat * (-sinYaw * cosRoll + cosYaw * sinPitch * sinRoll) - sinLat * cosPitch * sinRoll;
        double a_33 = cosLat * (sinYaw * sinRoll + cosYaw * sinPitch * cosRoll) - sinLat * cosPitch * cosRoll;

        return atan2(a_23, a_33);
    };



    double EulerConversions::toDegrees(double radian)
    {
         return (radian *(180/M_PI));
    }

    

}