aman-es/src/types/Inbound.cpp

/*
 * Author:
 *   Sven Czarnian <devel@svcz.de>
 * Brief:
 *   Implements the inbound
 * Copyright:
 *   2021 Sven Czarnian
 * License:
 *   GNU General Public License v3 (GPLv3)
 */

#include <Windows.h>

#include <gsl/gsl>

#include <aman/helper/String.h>
#include <aman/types/Inbound.h>

using namespace aman;

static __inline GeoCoordinate __convert(const EuroScopePlugIn::CPosition& position) {
    return GeoCoordinate(static_cast<float>(position.m_Longitude) * degree, static_cast<float>(position.m_Latitude) * degree);
}

Inbound::Inbound(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound,
                 const google::protobuf::RepeatedPtrField<aman::WindData>& wind) :
        m_windLevels(),
        m_windDirections(),
        m_windSpeeds(),
        m_performanceData(),
        m_fixedPlan(inbound.fixed()),
        m_star(inbound.arrivalroute()),
        m_runway(inbound.arrivalrunway()),
        m_nextStarWaypoint(),
        m_arrivalRoute(),
        m_timeToLose() {
    this->createWindTables(wind);
    this->updatePrediction(target, inbound, true);
    auto flightplan = target.GetCorrelatedFlightPlan();
    this->update(flightplan);
}

void Inbound::createWindTables(const google::protobuf::RepeatedPtrField<aman::WindData>& wind) {
    this->m_windLevels.clear();
    this->m_windDirections.clear();
    this->m_windSpeeds.clear();

    this->m_windLevels.reserve(static_cast<std::size_t>(wind.size()));
    this->m_windDirections.reserve(static_cast<std::size_t>(wind.size()));
    this->m_windSpeeds.reserve(static_cast<std::size_t>(wind.size()));

    for (int i = 0; i < wind.size(); ++i) {
        const auto& level = wind.Get(i);

        this->m_windLevels.push_back(static_cast<float>(level.altitude()));
        this->m_windDirections.push_back(static_cast<float>(level.direction()));
        this->m_windSpeeds.push_back(static_cast<float>(level.speed()));
    }
}

void Inbound::updatePrediction(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound, bool forceUpdate) {
    bool updatedFlightplan = false;

    this->m_performanceData.speedAboveFL240 = static_cast<float>(inbound.performance().iasabovefl240()) * knot;
    this->m_performanceData.speedAboveFL100 = static_cast<float>(inbound.performance().iasabovefl100()) * knot;
    this->m_performanceData.speedBelowFL100 = static_cast<float>(inbound.performance().iasbelowfl100()) * knot;
    this->m_performanceData.speedApproach = static_cast<float>(inbound.performance().iasapproach()) * knot;

    if (true == forceUpdate || this->m_star != target.GetCorrelatedFlightPlan().GetFlightPlanData().GetStarName() || this->m_runway != target.GetCorrelatedFlightPlan().GetFlightPlanData().GetArrivalRwy()) {
        std::string route(target.GetCorrelatedFlightPlan().GetFlightPlanData().GetRoute());
        std::string arrival = this->m_star + "/" + this->m_runway;

        auto split = String::splitString(route, " ");
        std::string newRoute;

        /* create the new route */
        if (1 < split.size()) {
            for (std::size_t i = 0; i < split.size() - 1; ++i)
                newRoute += gsl::at(split, i) + " ";
        }
        /* check if the last entry is the arrival route */
        const auto& lastEntry = gsl::at(split, split.size() - 1);
        if (lastEntry.cend() == std::find_if(lastEntry.cbegin(), lastEntry.cend(), ::isdigit))
            newRoute += gsl::at(split, split.size() - 1) + " ";
        /* add the arrival route */
        newRoute += arrival;

        /* write into the flight plan */
        target.GetCorrelatedFlightPlan().GetFlightPlanData().SetRoute(newRoute.c_str());
        target.GetCorrelatedFlightPlan().GetFlightPlanData().AmendFlightPlan();

        updatedFlightplan = true;
    }

    if (true == updatedFlightplan) {
        this->m_arrivalRoute.clear();
        this->m_arrivalRoute.reserve(inbound.waypoints_size());
        auto route = target.GetCorrelatedFlightPlan().GetExtractedRoute();
        int lastExtractedIndex = 0;

        for (int i = 0; i < inbound.waypoints_size(); ++i) {
            const auto& plannedPoint = inbound.waypoints(i);

            const auto pta = UtcTime::stringToTime(plannedPoint.pta());
            const auto altitude = static_cast<float>(plannedPoint.altitude()) * feet;
            const auto ias = static_cast<float>(plannedPoint.indicatedairspeed()) * knot;
            GeoCoordinate coordinate;

            bool found = false;
            for (int c = lastExtractedIndex; c < route.GetPointsNumber(); ++c) {
                if (route.GetPointName(c) == plannedPoint.name()) {
                    coordinate = __convert(route.GetPointPosition(c));
                    lastExtractedIndex = c;
                    found = true;
                    break;
                }
            }

            if (true == found)
                this->m_arrivalRoute.push_back(ArrivalWaypoint(plannedPoint.name(), coordinate, altitude, ias, pta));
        }
    }
}

void Inbound::update(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound,
                     const google::protobuf::RepeatedPtrField<aman::WindData>& wind) {
    this->m_fixedPlan = inbound.fixed();
    this->m_star = inbound.arrivalroute();
    this->m_runway = inbound.arrivalrunway();
    this->createWindTables(wind);

    this->updatePrediction(target, inbound, false);
    auto flightplan = target.GetCorrelatedFlightPlan();
    this->update(flightplan);
}

Velocity Inbound::indicatedAirspeed(const Length& altitude) const noexcept {
    if (24000_ft <= altitude)
        return this->m_performanceData.speedAboveFL240;
    else if (10000_ft <= altitude)
        return this->m_performanceData.speedAboveFL100;
    else if (1000_ft < altitude)
        return this->m_performanceData.speedBelowFL100;
    else
        return this->m_performanceData.speedApproach;
}

template <typename T, typename U>
static __inline U __interpolate(const std::vector<T>& xAxis, const std::vector<U>& yAxis, const T& xValue, bool limit) {
    bool inverse = gsl::at(xAxis, 0) > gsl::at(xAxis, xAxis.size() - 1);

    /* define the search value */
    T value = xValue;
    if (true == limit) {
        if (true == inverse) {
            if (value > gsl::at(xAxis, 0))
                value = gsl::at(xAxis, 0);
            else if (value < gsl::at(xAxis, xAxis.size() - 1))
                value = gsl::at(xAxis, xAxis.size() - 1);
        }
        else {
            if (value < gsl::at(xAxis, 0))
                value = gsl::at(xAxis, 0);
            else if (value > gsl::at(xAxis, xAxis.size() - 1))
                value = gsl::at(xAxis, xAxis.size() - 1);
        }
    }

    /* search the correct value */
    for (std::size_t i = 0; i < xAxis.size() - 1; ++i) {
        const auto& prevX = gsl::at(xAxis, i);
        const auto& nextX = gsl::at(xAxis, i + 1);
        bool inside;

        if (true == inverse)
            inside = prevX >= xValue && nextX <= xValue;
        else
            inside = prevX <= xValue && nextX >= xValue;

        if (true == inside) {
            auto ratio = (xValue - prevX) / (nextX - prevX);
            const auto& prevY = gsl::at(yAxis, i);
            const auto& nextY = gsl::at(yAxis, i + 1);
            return prevY + ratio * (nextY - prevY);
        }
    }

    return U();
}

Velocity Inbound::groundSpeed(const Length& altitude, const Velocity& ias, const Angle& heading) {
    static std::vector <float> levels = {
        50000.0f, 45000.0f, 40000.0f, 38000.0f, 36000.0f, 34000.0f, 32000.0f, 30000.0f, 28000.0f,
        26000.0f, 24000.0f, 22000.0f, 20000.0f, 18000.0f, 16000.0f, 15000.0f, 14000.0f, 13000.0f,
        12000.0f, 11000.0f, 10000.0f, 9000.0f, 8000.0f, 7000.0f, 6000.0f, 5000.0f, 4000.0f,
        3000.0f, 2000.0f, 1000.0f, 0.0f
    };
    static std::vector<float> densities = {
        0.18648f, 0.23714f, 0.24617f, 0.33199f, 0.36518f, 0.39444f, 0.42546f, 0.45831f, 0.402506f, 0.432497f, 0.464169f,
        0.60954f, 0.65269f, 0.69815f, 0.74598f, 0.77082f, 0.79628f, 0.82238f, 0.84914f, 0.87655f, 0.90464f, 0.93341f,
        0.96287f, 0.99304f, 1.02393f, 1.05555f, 1.08791f, 1.12102f, 1.1549f, 1.18955f, 1.225f
    };

    const auto density = __interpolate(levels, densities, altitude.convert(feet), true);
    const auto tas = ias * std::sqrtf(1.225f / density);
    Angle windDirection(0.0_deg);
    Velocity windSpeed(0.0_mps);

    if (0 != this->m_windLevels.size()) {
        windDirection = __interpolate(this->m_windLevels, this->m_windDirections, altitude.convert(feet), true) * degree;
        windSpeed = __interpolate(this->m_windLevels, this->m_windSpeeds, altitude.convert(feet), true) * knot;
    }

    return tas + windSpeed * std::cosf(windDirection.convert(radian) - heading.convert(radian));
}

static __inline Angle __normalize(const Angle& angle) {
    auto retval(angle);

    while (-1.0f * 180_deg > retval)
        retval += 360_deg;
    while (180_deg < retval)
        retval -= 360_deg;

    return retval;
}

int Inbound::findIndexInPredictedPath(const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions, const GeoCoordinate& position) {
    if (0 == predictions.GetPointsNumber())
        return 0;

    GeoCoordinate lastPosition(__convert(predictions.GetPosition(0)));

    for (int i = 1; i < predictions.GetPointsNumber(); ++i) {
        GeoCoordinate coordinate(__convert(predictions.GetPosition(i)));

        const auto prev = lastPosition.bearingTo(position);
        const auto next = coordinate.bearingTo(position);
        const auto delta = __normalize(prev - next);
        if (100_deg <= delta.abs())
            return i;

        lastPosition = coordinate;
    }

    return predictions.GetPointsNumber();
}

void Inbound::update(EuroScopePlugIn::CRadarTarget& target) {
    if (this->m_arrivalRoute.size() <= this->m_nextStarWaypoint) {
        this->m_timeToLose = 0_s;
        return;
    }

    const auto& destination = gsl::at(this->m_arrivalRoute, this->m_nextStarWaypoint);
    const auto& predictions = target.GetCorrelatedFlightPlan().GetPositionPredictions();
    GeoCoordinate lastPosition(__convert(target.GetPosition().GetPosition()));
    Length distanceToNextWaypoint = 0_m;

    /* calculate the distance to the correct waypoint */
    for (int i = 0; i < predictions.GetPointsNumber(); ++i) {
        GeoCoordinate coordinate(__convert(predictions.GetPosition(i)));

        const auto prev = lastPosition.bearingTo(destination.position());
        const auto next = coordinate.bearingTo(destination.position());
        const auto delta = __normalize(prev - next);
        if (100_deg <= delta.abs())
            break;

        distanceToNextWaypoint += coordinate.distanceTo(lastPosition);
        lastPosition = coordinate;
    }

    /* predict the flight and the descend */
    Velocity groundSpeed = static_cast<float>(target.GetPosition().GetReportedGS()) * knot;
    Length altitude = static_cast<float>(target.GetPosition().GetFlightLevel()) * feet;
    Angle heading = __convert(predictions.GetPosition(0)).bearingTo(destination.position());

    Time flightTime = 0_s;
    while (0.0_m < distanceToNextWaypoint) {
        Length distance = groundSpeed * 1_s;

        if (altitude > destination.altitude()) {
            /* new descend required based on 3° glide */
            if (((altitude - destination.altitude()).convert(feet) / 1000.0f * 3.0f) > distanceToNextWaypoint.convert(nauticmile)) {
                const auto oldGS = groundSpeed;
                const auto descendRate = oldGS * 1_s * std::sinf(0.0523599f);
                altitude -= descendRate;

                const auto newGS = this->groundSpeed(altitude, this->indicatedAirspeed(altitude), heading);
                groundSpeed = std::min(groundSpeed, newGS);

                distance = (groundSpeed + oldGS) * 0.5f * 11_s;
            }
        }

        distanceToNextWaypoint -= distance;
        flightTime += 1_s;
    }

    auto currentUtc = UtcTime::currentUtc();
    auto pta = UtcTime::timeToString(destination.plannedArrivalTime());
    auto estimated = UtcTime::timeToString(currentUtc + std::chrono::seconds(static_cast<int>(flightTime.convert(second))));
    auto delta = std::chrono::duration_cast<std::chrono::seconds>(destination.plannedArrivalTime() - currentUtc);
    auto plannedFlightTime = static_cast<float>(delta.count()) * second;
    this->m_timeToLose = plannedFlightTime - flightTime;
}

void Inbound::update(EuroScopePlugIn::CFlightPlan& plan) {
    this->m_nextStarWaypoint = 0;
    if (0 == this->m_arrivalRoute.size())
        return;

    /* find the point on the route */
    std::string_view direct(plan.GetControllerAssignedData().GetDirectToPointName());
    auto route = plan.GetExtractedRoute();
    int starEntry = route.GetPointsNumber(), directEntry = route.GetPointsNumber();

    /* TODO search point if direct is empty */

    for (int c = 0; c < route.GetPointsNumber(); ++c) {
        std::string_view waypointName(route.GetPointName(c));

        if (waypointName == this->m_arrivalRoute.front().name())
            starEntry = c;
        else if (waypointName == direct)
            directEntry = c;
    }

    /* search the direct to entry */
    if (directEntry > starEntry && directEntry < route.GetPointsNumber()) {
        /* try to find the closest next waypoint */
        while (0 == this->m_nextStarWaypoint) {
            for (std::size_t i = 0; i < this->m_arrivalRoute.size(); ++i) {
                if (direct == gsl::at(this->m_arrivalRoute, i).name()) {
                    this->m_nextStarWaypoint = i;
                    break;
                }
            }

            if (0 == this->m_nextStarWaypoint) {
                directEntry += 1;
                if (directEntry >= route.GetPointsNumber()) {
                    this->m_nextStarWaypoint = this->m_arrivalRoute.size() - 1;
                    break;
                }

                direct = std::string_view(route.GetPointName(directEntry));
            }
        }
    }

    EuroScopePlugIn::CRadarTarget target = plan.GetCorrelatedRadarTarget();
    this->update(target);
}

bool Inbound::fixedPlan() const noexcept {
    return this->m_fixedPlan;
}

const Time& Inbound::timeToLose() const noexcept {
    return this->m_timeToLose;
}