EntityLibrary/include/Entities/Movement.hpp

#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();
             /**
            * @brief Movement constructor
            * @param std::shared_ptr<SimCore::Position> pos
            */
            Movement(std::shared_ptr<SimCore::Position> pos);
            
            /**
            * @brief Movement constructor
            * @param std::shared_ptr<SimCore::Position> pos
            * @param double course in Degree
            */
            Movement(std::shared_ptr<SimCore::Position> pos, double course);

            /**
            * @brief Movement constructor
            * @param std::shared_ptr<SimCore::Position> pos
            * @param double course in Degree
            * @param double speed in m/s
            */
            Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed);

            /**
            * @brief Movement constructor
            * @param std::shared_ptr<SimCore::Position> pos
            * @param double course in Degree
            * @param double speed in m/s
            * @param double sppitch (climbangle) in Degree
            */
            Movement(std::shared_ptr<SimCore::Position> pos, double course, double speed, double pitch);
            
            /**
            * @brief Movement constructor
            * @param std::shared_ptr<SimCore::Position> pos
            * @param Eigen::Vector3d ECEF vector
            */
            Movement(std::shared_ptr<SimCore::Position> pos, Eigen::Vector3d ECEF);

            

            // ~Movement();

            void updatePosition(double dt);

            void updatePositionSimple(double dt);


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

            /**
            * @brief set the course
            * @param double course in degree
            */
            void setCourse(double course);

            /**
            * @brief set the course
            * @param double course in rad
            */
            void setCourseInRad(double courseInRad);


            /**
            * @brief returns the speed in m/s
            *
            */
            double getSpeed();
       
            /**
            * @brief set the speed in m/s
            * @param set speed in m/s
            */
            void setSpeed(double speed);

            /**
            * @brief set the speed in knots
            * @param set speed in knots
            */
            void setSpeedInKnots(double speed);


            /**
            * returns the climbing angle in rad of the entity of the entity
            *
            */
            double getPitch();
            
            /**
            * @brief set the course
            * @param double pitch /climbangle in degree
            */
            void setPitch(double pitch);

            /**
            * @brief set the course
            * @param double pitch /climbangle in rad
            */
            void setPitchInRad(double pitch);

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

            std::shared_ptr<SimCore::Position> getPosition();

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

            void setPosition(SimCore::Position pos);

            /**
            * @return returns copy of orientation class
            *
            */
            SimCore::Orientation getownOrientation();

            /**
            * @return Eigen::Vector3d  of the current eulerangles
            *
            */
            Eigen::Vector3d getEulerAngles();

            // Eigen::Matrix3d getRotationMatrix(double lat, double lon);


            /**
            * @brief transforms from local ENU (East North Up) Vector to ECEF (Earth Centred Earth Fixed)  
            * @param Eigen::Vector3d ENU 
            * @param SimCore::Position Position
            * @return Eigen::Vector3d ECEF 
            * from https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
            */
           static  Eigen::Vector3d ToECEFFromENU(Eigen::Vector3d ENU, SimCore::Position position);


            /**
            * @brief transforms Course, Speed and ClimbAngle to local ENU (East North Up) Vector   
            * @param double course 
            * @param double speed 
            * @param double climbAngle 
            * @return Eigen::Vector3d ENU 
            */
            static Eigen::Vector3d ENUFromCourseSpeedClbToLocalSpeedVector(double course, double speed, double climbAngle);

            /**
            * @brief transforms get ENU (East North Up) from ECEF Vector
            * @param Eigen::Vector3d ECEF  
            * @param SimCore::Position position 
            * @return Eigen::Vector3d ENU 
            */
            static Eigen::Vector3d ENUFromECEF(Eigen::Vector3d ECEF, SimCore::Position position);

            /**
            * @brief converts ENU (East North Up) velocity Vector to course speed and climbangles
            * @param Eigen::Vector3d ENU
            * @return std::tuple<double, double, double> course speed, climbangle
            */
            static std::tuple<double, double, double> getCourseSpeedClimbAngleFromENU(Eigen::Vector3d ENU);

        private:
            std::shared_ptr<SimCore::Position> ownPosition_ = nullptr;
            SimCore::Orientation ownOrientation_;
            
            Eigen::Vector3d ToECEFFromENU();

            Eigen::Vector3d ENUFromCourseSpeedClbToLocalSpeedVector();
    
            /// East North Up Vector wich represents the local Speed Vector
            Eigen::Vector3d ENUVec_;

            Eigen::Vector3d accelerationVec_;


            double course_;
            double speed_;
            double climbAngle_;

            const double precision = 0.01;

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


    };
}