// SwiftAircraftManager.cpp - Manager class for aircrafts generated by swift
//
// Copyright (C) 2019 - swift Project Community / Contributors (http://swift-project.org/)
// Adapted to Flightgear by Lars Toenning <dev@ltoenning.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

#include "SwiftAircraftManager.h"
#include "SwiftAircraft.h"
#include <Main/globals.hxx>
#include <utility>

FGSwiftAircraftManager::FGSwiftAircraftManager()
= default;

FGSwiftAircraftManager::~FGSwiftAircraftManager()
= default;

bool FGSwiftAircraftManager::addPlane(const std::string& callsign, std::string modelString)
{
    if (aircraftByCallsign.find(callsign) != aircraftByCallsign.end())
        return false;

    const char* typeString = "swift";
    SGPropertyNode_ptr root = globals->get_props()->getNode("ai/models",true);
    SGPropertyNode_ptr p;
    int i;
    for(i = 0; i < 10000; i++){
        p = root->getNode(typeString,i,false);

        if(!p || !p->getBoolValue("valid",false))
            break;
    }
    p = root->getNode(typeString,i,true);
    p->setIntValue("id",i);
    auto curAircraft = new FGSwiftAircraft(callsign, std::move(modelString), p);
    aircraftByCallsign.insert(std::pair<std::string, FGSwiftAircraft*>(callsign, curAircraft));
    return true;
}

void FGSwiftAircraftManager::updatePlanes(std::vector<std::string> callsigns, std::vector<SGGeod> positions, std::vector<SGVec3d> orientations, std::vector<double> groundspeeds, std::vector<bool> onGrounds)
{
	for (long unsigned int i = 0; i < callsigns.size(); i++) {
        auto it = aircraftByCallsign.find(callsigns.at(i));
		if (it != aircraftByCallsign.end()) {
            it->second->updatePosition(positions.at(i), orientations.at(i), groundspeeds.at(i),true);
		}
	}

}

void FGSwiftAircraftManager::getRemoteAircraftData(std::vector<std::string>& callsigns, std::vector<double>& latitudesDeg, std::vector<double>& longitudesDeg, std::vector<double>& elevationsM, std::vector<double>& verticalOffsets) const
{
    if (callsigns.empty() || aircraftByCallsign.empty()) { return; }

    const auto requestedCallsigns = callsigns;
    callsigns.clear();
    latitudesDeg.clear();
    longitudesDeg.clear();
    elevationsM.clear();
    verticalOffsets.clear();

	for (const auto & requestedCallsign : requestedCallsigns) {
        const auto it = aircraftByCallsign.find(requestedCallsign);
        if(it == aircraftByCallsign.end()) { continue; }

        const FGSwiftAircraft *aircraft = it->second;
        assert(aircraft);


        SGGeod pos;
        pos.setLatitudeDeg(aircraft->getLatDeg());
        pos.setLongitudeDeg(aircraft->getLongDeg());
        const double latDeg = pos.getLatitudeDeg();
        const double lonDeg = pos.getLongitudeDeg();
        double groundElevation = aircraft->getGroundElevation(pos);

        double fudgeFactor     = aircraft->getFudgeFactor();
        (void)fudgeFactor;
        callsigns.push_back(requestedCallsign);
        latitudesDeg.push_back(latDeg);
        longitudesDeg.push_back(lonDeg);
        elevationsM.push_back(groundElevation);
        verticalOffsets.push_back(0);
	}
}

void FGSwiftAircraftManager::removePlane(const std::string& callsign)
{
    auto it = aircraftByCallsign.find(callsign);
    if (it != aircraftByCallsign.end()) {
		delete it->second;
        aircraftByCallsign.erase(it);
    }
}

void FGSwiftAircraftManager::removeAllPlanes()
{
    for (auto it = aircraftByCallsign.begin(); it != aircraftByCallsign.end(); ++it) {
        delete it->second;
        aircraftByCallsign.erase(it);
    }
}

double FGSwiftAircraftManager::getElevationAtPosition(const std::string &callsign, const SGGeod& pos) const
{
    auto it = aircraftByCallsign.find(callsign);
    if(it != aircraftByCallsign.end()){
        return it->second->getGroundElevation(pos);
    }
    return std::numeric_limits<double>::quiet_NaN();
}
