#include "SimCore/SimCore.hpp"
#include <SimCore/Position.hpp>


namespace SimCore {

    Position::Position():mx()
    {
        earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
        geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
        wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
        
        valid_ = false;
    }
    Position::Position(double X, double Y, double Z):mx()
    {
        earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
        geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
        wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
        setGeocentricPos( X, Y,  Z);

    }


    Position::Position(const Position& other){
                    std::lock_guard<std::mutex> lock(other.mx);
                    GeocentricPos_ = other.GeocentricPos_;
                    GeodesicPos_ = other.GeodesicPos_;

                    earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
                    geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
                    wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
        valid_ = true;
    }

    Eigen::Vector3d Position::getGeocentricPos()
    {
        std::lock_guard<std::mutex> lock(mx);

        return GeocentricPos_;

    }

    void Position::getGeocentricPos(double &lat, double &lon,double &h)
    {
        std::lock_guard<std::mutex> lock(mx);
        lat = GeodesicPos_[LATITUDE];     
        lon = GeodesicPos_[LONGITUDE];
        h = GeodesicPos_[HEIGHT];

    }


    Eigen::Vector3d Position::getGeodesicPos()
    {
        std::lock_guard<std::mutex> lock(mx);

        return GeodesicPos_;
    }

    void Position::setGeocentricPos(double X, double Y, double Z){
        std::lock_guard<std::mutex> lock(mx);

        GeocentricPos_(GeocentricPosition::X) = X; 
        GeocentricPos_(GeocentricPosition::Y) = Y;     
        GeocentricPos_(GeocentricPosition::Z)= Z;     
        
        double lat, lon, h;


        earth_->Reverse(GeocentricPos_(GeocentricPosition::X),GeocentricPos_(GeocentricPosition::Y),GeocentricPos_(GeocentricPosition::Z),GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT));
        valid_ = true;
    }   

    void Position::setGeodesicPos(double lat, double lon, int height){
        std::lock_guard<std::mutex> lock(mx);

        GeodesicPos_(GeodesicPosition::LATITUDE) = lat;
        GeodesicPos_(GeodesicPosition::LONGITUDE) = lon;
        GeodesicPos_(GeodesicPosition::HEIGHT) = height;
        earth_->Forward( GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT),GeocentricPos_(X),GeocentricPos_(Y),GeocentricPos_(Z));
       
        valid_ = true;
    }

 
    std::tuple<double, double> Position::distanceBearingToPosition(Position &p2)
    {
        if (valid_) {
            std::lock_guard<std::mutex> lock(mx);
            double s12, azi1, azi2, m12;
            geod_->Inverse(this->GeodesicPos_[LATITUDE],
                        this->GeodesicPos_[LONGITUDE],
                        p2.GeodesicPos_[LATITUDE],
                        p2.GeodesicPos_[LONGITUDE],
                        s12,azi1,azi2);

            return {s12,azi1};
        }else 
        {
            return {0,0};
        }
                            
    }

     bool Position::isValid()
     {
        return valid_;
     }




    bool Position::operator==( Position &lhs)
    {
        std::lock_guard<std::mutex> lock(mx);

        if (this->GeocentricPos_ == lhs.getGeocentricPos()) {
            return true;
        }else{
            return false;
        }

    }

    Position& Position::operator=(const Position other)
    {
        std::lock_guard<std::mutex> lock(other.mx);
        GeocentricPos_ = other.GeocentricPos_;
        GeodesicPos_ = other.GeodesicPos_;
        
        return *this;
    }



}


