#include "SimCore/EulerConversion.hpp"
#include "SimCore/SimCore.hpp"
#include <SimCore/Orientation.hpp>


namespace SimCore 
{

    Orientation::Orientation(double heading, double pitch , double roll ):heading_(heading),pitch_(pitch),roll_(roll)
    {
    }


    Orientation::Orientation(Eigen::Vector3d angles, double lat, double lon)
    {
        heading_ = EulerConversions::getOrientationFromEuler( lat,  lon,  angles[PSI],angles[THETA] );
        pitch_ =  EulerConversions::getPitchFromEuler(lat, lon, angles[PSI],angles[THETA]);
        roll_ = EulerConversions::getRollFromEuler( lat,  lon,  angles[PSI],angles[THETA],angles[PHI] );
    }



    void Orientation::setHeading(double heading)
    {
        heading_ = heading;
    }

    double Orientation::getHeading()
    {
    return heading_;
    }

    void Orientation::setPitch(double pitch)
    {
    pitch_ = pitch;
    }

    double Orientation::getPitch()
    {
    return pitch_;
    }

    void Orientation::setRoll(double roll)
    {
    roll_ = roll;
    }

    double Orientation::getRoll()
    {
        return roll_;
    }

    Eigen::Vector3d Orientation::getEulerAngles(double lat ,double lon)
    {
        eulerAngels_[PHI]  =  EulerConversions::getPhiFromTaitBryanAngles( lat,  lon,  heading_, pitch_, roll_);
        eulerAngels_[THETA] = EulerConversions::getThetaFromTaitBryanAngles(lat, lon, heading_, pitch_);
        eulerAngels_[PSI]  = EulerConversions::getPsiFromTaitBryanAngles(lat, lon, heading_, pitch_);

        return eulerAngels_;

    }

    

}