#include <SimCore/Messages/RadarTrack.hpp>
#include <SimCore/SimCore.hpp>
#include <Entities/Tracklist/TracklistItem.hpp>

#include <algorithm>
#include <memory>
#include <chrono>
#include <tuple>

namespace TrackList {

    TracklistItem::TracklistItem(std::shared_ptr<SimCore::Track> track,SensorData sensorData):trackID_(track->getIdentifier())
    {

        updateTrack(track,sensorData);
        addSensorDataToSensorList(sensorData);
        
    }

      SimCore::Identifier TracklistItem::getID()
     {
        return trackID_;
     }

    void TracklistItem::setPosition(SimCore::Position position)
    {
        position_ = position;
    }

    SimCore::Position TracklistItem::getPosition()
    {
        return position_;
    }

    void TracklistItem::setSpeed(double speed)
    {
        speed_ = speed;
    }

    double TracklistItem::getSpeed()
    {
        return speed_;
    }

    void TracklistItem::setCourse(double course)
    {
        course_ = course;
    }

    double TracklistItem::getCourse()
    {
        return course_;;
    }

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

    double TracklistItem::getpitch()
    {
        return pitch_;
    }

    double TracklistItem::getBearing()
    {
        return bearing_;
    }
    double TracklistItem::getRange()
    {
        return range_;
    }

    SimCore::ObjectSource TracklistItem::getObjectSource()
    {
        return ObjectSource_;
    }

    std::chrono::time_point<std::chrono::steady_clock> TracklistItem::getLastUpdateTimestamp()
    {
        return lastUpdateTimestamp_;
    }


    void TracklistItem::updateTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData)
    {   
        
        auto trackKind = track->getTrackkind();
        if (trackKind == SimCore::TrackKind::RADAR_TRACK) {
            std::shared_ptr<SimCore::RadarTrack> radarTrack = std::dynamic_pointer_cast<SimCore::RadarTrack>(track);

            position_ = radarTrack->getPostion();
            range_ = radarTrack->getRange();
            bearing_ = radarTrack->getBearing();
            course_ = radarTrack->getCourse();
            speed_ = radarTrack->getSpeed();
            lastUpdateTimestamp_ =  std::chrono::steady_clock::now();


        }

        if (isSensorinSensorlist(sensorData) != true) {
            addSensorDataToSensorList(sensorData);
        }

    }


    bool TracklistItem::isSensorinSensorlist(SensorData sensorData)
    {
        
        auto it =  std::find(SensorList.begin(),SensorList.end(), sensorData);

        if (it != SensorList.end()) {
            return true;
        } else {
            return false;
        }

    }


    bool TracklistItem::isSensorIDKnown(SimCore::Identifier SensorID)
    {
        for (auto i: SensorList) {
            if (i.sensorID == SensorID)
            {
                return true;

            }
        }
        return false;
    }


    void TracklistItem::addSensorDataToSensorList(SensorData sensorData)
    {

        if (isSensorinSensorlist(sensorData) == false) {

           SensorList.emplace_back(sensorData);

        } 
    }



    bool TracklistItem::checkIfSensorIDIsIn(SimCore::Identifier SensorID)
    {
        for (auto i:SensorList) {

            if (i.sensorID == SensorID) {
                return true;
            }
        }
        return false;
    }

}