#pragma once

#include "SimCore/SimCore.hpp"
#include <Eigen/Core>
#include <GeographicLib/Ellipsoid.hpp>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/GeodesicLine.hpp>
#include <GeographicLib/Constants.hpp>
#include <atomic>
#include <memory>
#include <mutex>

namespace SimCore {
    class Position {
        public: 
            Position();
            Position(double X, double Y, double Z);
            
            Position(const Position& other);

            /**
            * @brief returns a eigen vector3d with the X, Y, Z coordinates
            * @return Eigen::Vector3d
            */
            Eigen::Vector3d getGeocentricPos();
            void getGeocentricPos(double &lat, double &lon,double &h);

            /**
            * @brief returns a eigen vector3d with the lat, lon, height coordinates
            * @return Eigen::Vector3d
            */
            Eigen::Vector3d getGeodesicPos();

            void setGeocentricPos(double X, double Y, double Z);
            void setGeodesicPos(double lat, double lon, int height);

            bool isValid();

            /**
            * @brief calculates the disatance and the bearing from this->position to the other position 
            *
            * @param Position  
            *
            * @return std::tuple<double distance, double bearing> 
            */
            std::tuple<double, double> distanceBearingToPosition(Position &p2);


            bool operator== ( Position &lhs);
            Position& operator=(const Position other); 

           


        private:
            std::shared_ptr<GeographicLib::Geocentric> earth_ = nullptr;          
            std::shared_ptr<GeographicLib::Geodesic> geod_ = nullptr;   
            std::shared_ptr<GeographicLib::Ellipsoid> wgs84_ = nullptr; 
            
            Eigen::Vector3d GeodesicPos_; // lat long height         
            Eigen::Vector3d GeocentricPos_; ///x y z 

            mutable std::mutex mx;

            std::atomic_bool valid_;

    };

}