#pragma once


#include <SimCore/Orientation.hpp>
#include <SimCore/Position.hpp>
#include <condition_variable>
#include <cstdint>
#include <Eigen/Core>
#include <loguru.hpp>


namespace Entities 
{

       

   

    

  


    class Movement
    {   
        public:
            Movement();
            Movement(SimCore::Position pos);
            Movement(SimCore::Position pos, double course);
            Movement(SimCore::Position pos, double course, double speed);
            Movement(SimCore::Position pos, double course, double speed, double pitch);

            

            // ~Movement();

            void updatePosition(double dt);


            /**
            * @brief returns the current Course representing the direction the front of the entity is pointing 
            *
            */
            double getCourse();

            void setCourse(double course);

            /**
            * @brief returns the speed in m/s
            *
            */
            double getSpeed();

            void setSpeed(double speed);

            /**
            * returns the climbing angle of the entity of the entity
            *
            */
            double getPitch();

            void setPitch(double pitch);

            /**
            * @brief returns the kinematicVector of Entity
            *
            */
            Eigen::Vector3d getKinematicVector();

            SimCore::Position getPosition();

            void setPosition(double lat, double lon, double height = 0);

            void setPosition(SimCore::Position pos);


            SimCore::Orientation getownOrientation();

            Eigen::Vector3d getEulerAngles();





        private:
            SimCore::Position ownPosition_;
            SimCore::Orientation ownOrientation_;

            Eigen::Vector3d kinematicVec_;
            Eigen::Vector3d accelerationVec_;


            double course_;
            double speed_;
            double pitch_;

            void calKinematicVector();
            Eigen::Matrix3d getRotationMatrix(double lat, double lon);


            mutable std::mutex mx;
            std::condition_variable c;


    };
}