/* Copyright (C) 2017
 * swift project Community / Contributors
 *
 * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level
 * directory of this distribution. No part of swift project, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 */

#include "interpolatorspline.h"
#include "interpolatorfunctions.h"
#include "blackmisc/network/fsdsetup.h"
#include "blackmisc/logmessage.h"
#include "blackmisc/verify.h"
#include "blackconfig/buildconfig.h"

using namespace BlackConfig;
using namespace BlackMisc::Aviation;
using namespace BlackMisc::Geo;
using namespace BlackMisc::Math;
using namespace BlackMisc::Network;
using namespace BlackMisc::PhysicalQuantities;
using namespace BlackMisc::Simulation;

namespace BlackMisc
{
    namespace Simulation
    {
        namespace
        {
            //! \private https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm
            template <size_t N>
            std::array<double, N> solveTridiagonal(std::array<std::array<double, N>, N> &matrix, std::array<double, N> &d)
            {
                // *INDENT-OFF*
                const auto a = [&matrix](size_t i) -> double& { return matrix[i][i-1]; }; // subdiagonal
                const auto b = [&matrix](size_t i) -> double& { return matrix[i][i  ]; }; // main diagonal
                const auto c = [&matrix](size_t i) -> double& { return matrix[i][i+1]; }; // superdiagonal

                // forward sweep
                c(0) /= b(0);
                d[0] /= b(0);
                for (size_t i = 1; i < N; ++i)
                {
                    const double denom = b(i) - a(i) * c(i - 1);
                    if (i < N-1) { c(i) /= denom; }
                    d[i] = (d[i] - a(i) * d[i - 1]) / denom;
                }

                // back substitution
                for (int i = N - 2; i >= 0; --i)
                {
                    const size_t it = static_cast<size_t>(i);
                    d[it] -= c(it) * d[it+1];
                }
                return d;
                // *INDENT-ON*
            }

            //! \private Linear equation expressed as tridiagonal matrix.
            //! https://en.wikipedia.org/wiki/Spline_interpolation
            //! http://blog.ivank.net/interpolation-with-cubic-splines.html
            template <size_t N>
            std::array<double, N> getDerivatives(const std::array<double, N> &x, const std::array<double, N> &y)
            {
                std::array<std::array<double, N>, N> a {{}};
                std::array<double, N> b {{}};

                // *INDENT-OFF*
                a[0][0] = 2.0 / (x[1] - x[0]);
                a[0][1] = 1.0 / (x[1] - x[0]);
                b[0]    = 3.0 * (y[1] - y[0]) / ((x[1] - x[0]) * (x[1] - x[0]));

                a[N-1][N-2] = 1.0 / (x[N-1] - x[N-2]);
                a[N-1][N-1] = 2.0 / (x[N-1] - x[N-2]);
                b[N-1]      = 3.0 * (y[N-1] - y[N-2]) / ((x[N-1] - x[N-2]) * (x[N-1] - x[N-2]));

                for (size_t i = 1; i < N - 1; ++i)
                {
                    a[i][i-1] = 1.0 / (x[i] - x[i-1]);
                    a[i][i  ] = 2.0 / (x[i] - x[i-1]) + 2.0 / (x[i+1] - x[i]);
                    a[i][i+1] = 1.0 / (x[i+1] - x[i]);
                    b[i]      = 3.0 * (y[i] - y[i-1]) / ((x[i] - x[i-1]) * (x[i] - x[i-1]))
                              + 3.0 * (y[i+1] - y[i]) / ((x[i+1] - x[i]) * (x[i+1] - x[i]));
                }
                // *INDENT-ON*

                solveTridiagonal(a, b);
                return b;
            }

            //! \private Cubic interpolation.
            double evalSplineInterval(double x, double x0, double x1, double y0, double y1, double k0, double k1)
            {
                const double t = (x - x0) / (x1 - x0);
                const double a =  k0 * (x1 - x0) - (y1 - y0);
                const double b = -k1 * (x1 - x0) + (y1 - y0);
                const double y = (1 - t) * y0 + t * y1 + t * (1 - t) * (a * (1 - t) + b * t);

                if (CBuildConfig::isLocalDeveloperDebugBuild())
                {
                    BLACK_VERIFY_X(t >= 0, Q_FUNC_INFO, "Expect t >= 0");
                    BLACK_VERIFY_X(t <= 1.0, Q_FUNC_INFO, "Expect t <= 1");
                }
                return y;
            }
        }

        bool CInterpolatorSpline::fillSituationsArray()
        {
            // m_s[0] .. oldest -> m_[2] .. latest
            // general idea, we interpolate from current situation -> latest situation

            // do we have the last interpolated situation?
            if (m_lastSituation.isNull())
            {
                if (m_currentSituations.isEmpty())
                {
                    // nothing we can do
                    m_s[0] = m_s[1] = m_s[2] = CAircraftSituation::null();
                    return false;
                }
                else
                {
                    // we start with the latest situation just to init the values
                    CAircraftSituation f = m_currentSituations.front();
                    f.setAdjustedMSecsSinceEpoch(m_currentTimeMsSinceEpoch); // adjusted time exactly "now"
                    m_s[0] = m_s[1] = m_s[2] = f;
                }
            }
            else
            {
                // in normal cases init some default values
                m_s[0] = m_s[1] = m_s[2] = m_lastSituation; // current position
            }

            // set some default values
            const qint64 defaultValueMs = CFsdSetup::c_interimPositionTimeOffsetMsec; // CLANG cannot use reference in qMax
            const qint64 os = qMax(defaultValueMs, m_s[2].getTimeOffsetMs());
            m_s[0].addMsecs(-os); // oldest, Ref T297 default offset time to fill data
            m_s[2].addMsecs(os);  // latest, Ref T297 default offset time to fill data
            if (m_currentSituations.isEmpty()) { return false; }

            // and use the real values if available
            // m_s[0] .. oldest -> m_[2] .. latest
            const CAircraftSituation latest = m_currentSituations.front();
            if (latest.isNewerThanAdjusted(m_s[1])) { m_s[2] = latest; }
            const qint64 currentAdjusted = m_s[1].getAdjustedMSecsSinceEpoch();

            // with https://dev.swift-project.org/T668#15841 avoid 2 very close positions
            // currently done by time, maybe we can also choose distance
            const qint64 osNotTooClose = qRound64(0.8 * os);
            const CAircraftSituation older = m_currentSituations.findObjectBeforeAdjustedOrDefault(currentAdjusted - osNotTooClose);
            if (!older.isNull())
            {
                m_s[0] = older;
            }
            else
            {
                const CAircraftSituation closeOlder = m_currentSituations.findObjectBeforeAdjustedOrDefault(currentAdjusted);
                if (!closeOlder.isNull()) { m_s[0] = closeOlder; }
            }
            const qint64 latestAdjusted = m_s[2].getAdjustedMSecsSinceEpoch();
            const qint64 olderAdjusted  = m_s[0].getAdjustedMSecsSinceEpoch();

            // not having a new situation itself is quite normal,
            // only if it persits it is critical.
            const bool hasNewer = latestAdjusted > m_currentTimeMsSinceEpoch;

            if (CBuildConfig::isLocalDeveloperDebugBuild())
            {
                const bool verified = this->verifyInterpolationSituations(m_s[0], m_s[1], m_s[2]); // oldest -> latest, only verify order
                if (!verified)
                {
                    static const QString vm("Unverified situations, m0-2 (oldest latest) %1 %2 %3");
                    const QString vmValues = vm.arg(olderAdjusted).arg(currentAdjusted).arg(latestAdjusted);
                    CLogMessage(this).warning(vmValues);
                    Q_UNUSED(vmValues);
                }
            }
            return hasNewer;
        }

        // pin vtables to this file
        void CInterpolatorSpline::anchor()
        { }

        CInterpolatorSpline::CInterpolant CInterpolatorSpline::getInterpolant(SituationLog &log)
        {
            // recalculate derivatives only if they changed
            const bool recalculate = (m_currentTimeMsSinceEpoch >= m_nextSampleAdjustedTime) || // new step
                                     (m_situationsLastModified  >  m_situationsLastModifiedUsed); // modified

            if (recalculate)
            {
                // with the latest updates of T243 the order and the offsets are supposed to be correct
                // so even mixing fast/slow updates shall work
                m_situationsLastModifiedUsed = m_situationsLastModified;
                const bool fillStatus = this->fillSituationsArray();
                if (!fillStatus)
                {
                    m_interpolant.setValid(false);
                    return  m_interpolant;
                }

                const std::array<std::array<double, 3>, 3> normals {{ m_s[0].getPosition().normalVectorDouble(), m_s[1].getPosition().normalVectorDouble(), m_s[2].getPosition().normalVectorDouble() }};
                PosArray pa;
                pa.x = {{ normals[0][0], normals[1][0], normals[2][0] }}; // oldest -> latest
                pa.y = {{ normals[0][1], normals[1][1], normals[2][1] }};
                pa.z = {{ normals[0][2], normals[1][2], normals[2][2] }}; // latest
                pa.t = {{ static_cast<double>(m_s[0].getAdjustedMSecsSinceEpoch()), static_cast<double>(m_s[1].getAdjustedMSecsSinceEpoch()), static_cast<double>(m_s[2].getAdjustedMSecsSinceEpoch()) }};

                pa.dx = getDerivatives(pa.t, pa.x);
                pa.dy = getDerivatives(pa.t, pa.y);
                pa.dz = getDerivatives(pa.t, pa.z);

                // - altitude unit must be the same for all three, but the unit itself does not matter
                // - ground elevantion here normally is not available
                // - some info how fast a plane moves: 100km/h => 1sec 27,7m => 5 secs 136m
                // - on an airport the plane does not move very fast, or not at all
                // - and the elevation remains (almost) constant for a wider area
                // - during flying the ground elevation not really matters
                this->updateElevations();
                static const CLengthUnit altUnit = CAltitude::defaultUnit();
                const CLength cg(this->getModelCG().switchedUnit(altUnit));
                const double a0 = m_s[0].getCorrectedAltitude(cg).value(altUnit); // oldest
                const double a1 = m_s[1].getCorrectedAltitude(cg).value(altUnit);
                const double a2 = m_s[2].getCorrectedAltitude(cg).value(altUnit); // latest
                pa.a = {{ a0, a1, a2 }};
                pa.gnd = {{ m_s[0].getOnGroundFactor(), m_s[1].getOnGroundFactor(), m_s[2].getOnGroundFactor() }};
                pa.da = getDerivatives(pa.t, pa.a);
                pa.dgnd = getDerivatives(pa.t, pa.gnd);

                m_prevSampleAdjustedTime = m_s[1].getAdjustedMSecsSinceEpoch();
                m_nextSampleAdjustedTime = m_s[2].getAdjustedMSecsSinceEpoch(); // latest
                m_prevSampleTime = m_s[1].getMSecsSinceEpoch(); // last interpolated situation normally
                m_nextSampleTime = m_s[2].getMSecsSinceEpoch(); // latest
                m_interpolant = CInterpolant(pa, altUnit, CInterpolatorPbh(m_s[1], m_s[2])); // older, newer
                Q_ASSERT_X(m_prevSampleAdjustedTime < m_nextSampleAdjustedTime, Q_FUNC_INFO, "Wrong time order");
            }

            // Example:
            // prev.sample time 5 (received at 0) , next sample time 10 (received at 5)
            // cur.time 6: dt1=6-5=1, dt2=5 => fraction 1/5
            // cur.time 9: dt1=9-5=4, dt2=5 => fraction 4/5
            //
            // we use different offset times for interim pos. updates
            // prev.sample time 5 (received at 0) , 7/r:5, 10 (rec. at 5)
            // cur.time 6: dt1=6-5=1, dt2=7-5 => fraction 1/2
            // cur.time 9: dt1=9-7=2, dt2=10-7=3 => fraction 2/3
            // we use different offset times for fast pos. updates
            // KB: is that correct with dt2, or would it be m_nextSampleTime - m_prevSampleTime
            //     as long as the offset time is constant, it does not matter
            const double dt1 = static_cast<double>(m_currentTimeMsSinceEpoch - m_prevSampleAdjustedTime);
            const double dt2 = static_cast<double>(m_nextSampleAdjustedTime  - m_prevSampleAdjustedTime);
            double timeFraction = dt1 / dt2;

            if (CBuildConfig::isLocalDeveloperDebugBuild())
            {
                BLACK_VERIFY_X(dt1 >= 0, Q_FUNC_INFO, "Expect postive dt1");
                BLACK_VERIFY_X(dt2 > 0, Q_FUNC_INFO, "Expect postive dt2");
                BLACK_VERIFY_X(isAcceptableTimeFraction(timeFraction), Q_FUNC_INFO, "Expect fraction 0-1");
            }
            timeFraction = clampValidTimeFraction(timeFraction);
            const qint64 interpolatedTime = m_prevSampleTime + qRound64(timeFraction * dt2);

            // time fraction is expected between 0-1
            m_currentInterpolationStatus.setInterpolated(true);
            m_interpolant.setTimes(m_currentTimeMsSinceEpoch, timeFraction, interpolatedTime);
            m_interpolant.setRecalculated(recalculate);

            if (this->doLogging())
            {
                log.interpolationSituations.clear();
                log.interpolationSituations.push_back(m_s[0]);
                log.interpolationSituations.push_back(m_s[1]);
                log.interpolationSituations.push_back(m_s[2]); // latest at end
                log.interpolator = 's';
                log.deltaSampleTimesMs = dt2;
                log.simTimeFraction = timeFraction;
                log.tsInterpolated = interpolatedTime; // without offsets
                log.interpolantRecalc = m_interpolant.isRecalculated();
            }

            return m_interpolant;
        }

        bool CInterpolatorSpline::updateElevations()
        {
            bool updated = false;
            for (unsigned int i = 0; i < m_s.size(); i++)
            {
                if (m_s[i].hasGroundElevation()) { continue; } // do not override existing values
                const CElevationPlane plane = this->findClosestElevationWithinRange(m_s[i], CElevationPlane::singlePointRadius());
                const bool u = m_s[i].setGroundElevationChecked(plane, CAircraftSituation::FromCache);
                updated |= u;
            }
            return updated;
        }

        bool CInterpolatorSpline::areAnyElevationsMissing() const
        {
            for (unsigned int i = 0; i < m_s.size(); i++)
            {
                if (!m_s[i].hasGroundElevation()) { return true; }
            }
            return false;
        }

        bool CInterpolatorSpline::isAnySituationNearGroundRelevant() const
        {
            for (unsigned int i = 0; i < m_s.size(); i++)
            {
                if (!m_s[i].canLikelySkipNearGroundInterpolation()) { return true; }
            }
            return false;
        }

        CInterpolatorSpline::CInterpolant::CInterpolant(const CInterpolatorSpline::PosArray &pa, const CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh) :
            m_pa(pa), m_altitudeUnit(altitudeUnit)
        {
            m_pbh = pbh;
            m_situationsAvailable = pa.size();
        }

        CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &currentSituation, bool interpolateGndFactor) const
        {
            const double t1 = m_pa.t[1];
            const double t2 = m_pa.t[2]; // latest (adjusted)

            bool valid = (t1 < t2) && (m_currentTimeMsSinceEpoc >= t1) && (m_currentTimeMsSinceEpoc < t2);
            if (!valid && CBuildConfig::isLocalDeveloperDebugBuild())
            {
                Q_ASSERT_X(t1 < t2, Q_FUNC_INFO, "Expect sorted times, latest first"); // that means a bug in our code init the values
                BLACK_VERIFY_X(m_currentTimeMsSinceEpoc >= t1, Q_FUNC_INFO, "invalid timestamp t1");
                BLACK_VERIFY_X(m_currentTimeMsSinceEpoc < t2, Q_FUNC_INFO, "invalid timestamp t2"); // t1==t2 results in div/0
            }
            if (!valid) { return CAircraftSituation::null(); }

            const double newX = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.x[1], m_pa.x[2], m_pa.dx[1], m_pa.dx[2]);
            const double newY = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.y[1], m_pa.y[2], m_pa.dy[1], m_pa.dy[2]);
            const double newZ = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.z[1], m_pa.z[2], m_pa.dz[1], m_pa.dz[2]);

            valid = CAircraftSituation::isValidVector(m_pa.x) && CAircraftSituation::isValidVector(m_pa.y) && CAircraftSituation::isValidVector(m_pa.z);
            if (!valid && CBuildConfig::isLocalDeveloperDebugBuild())
            {
                BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.x), Q_FUNC_INFO, "invalid X"); // all x values
                BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.y), Q_FUNC_INFO, "invalid Y"); // all y values
                BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.z), Q_FUNC_INFO, "invalid Z"); // all z values
            }
            if (!valid) { return CAircraftSituation::null(); }

            CAircraftSituation newSituation(currentSituation);
            const std::array<double, 3> normalVector = {{ newX, newY, newZ }};
            const CCoordinateGeodetic currentPosition(normalVector);

            valid = CAircraftSituation::isValidVector(normalVector);
            if (!valid && CBuildConfig::isLocalDeveloperDebugBuild())
            {
                BLACK_VERIFY_X(valid, Q_FUNC_INFO, "invalid vector");
                CLogMessage(this).warning(u"Invalid vector for '%1' v: %2 %3 %4") <<
                        currentSituation.getCallsign().asString() << normalVector[0] << normalVector[1] << normalVector[2];
            }
            if (!valid) { return CAircraftSituation::null(); }

            const double newA = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.a[1], m_pa.a[2], m_pa.da[1], m_pa.da[2]);
            const CAltitude alt(newA, m_altitudeUnit);

            newSituation.setPosition(currentPosition);
            newSituation.setAltitude(alt);
            newSituation.setMSecsSinceEpoch(this->getInterpolatedTime());

            if (interpolateGndFactor)
            {
                const double gnd1 = m_pa.gnd[1];
                const double gnd2 = m_pa.gnd[2]; // latest
                do
                {
                    newSituation.setOnGroundDetails(CAircraftSituation::OnGroundByInterpolation);
                    if (CAircraftSituation::isGfEqualAirborne(gnd1, gnd2)) { newSituation.setOnGround(false); break; }
                    if (CAircraftSituation::isGfEqualOnGround(gnd1, gnd2)) { newSituation.setOnGround(true); break; }
                    const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]);
                    newSituation.setOnGroundFactor(newGnd);
                    newSituation.setOnGroundFromGroundFactorFromInterpolation(groundInterpolationFactor());
                }
                while (false);
            }
            return newSituation;
        }

        void CInterpolatorSpline::CInterpolant::setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs)
        {
            m_currentTimeMsSinceEpoc = currentTimeMs;
            m_interpolatedTime = interpolatedTimeMs;
            m_pbh.setTimeFraction(timeFraction);
        }

        void CInterpolatorSpline::PosArray::initToZero()
        {
            for (uint i = 0; i < 3; i++)
            {
                x[i]  = 0; y[i] = 0; z[i] = 0;
                a[i]  = 0; t[i] = 0;
                dx[i] = 0; dy[i] = 0; dz[i] = 0;
                da[i] = 0;
                gnd[i] = 0; dgnd[i] = 0;
            }
        }

        const CInterpolatorSpline::PosArray &CInterpolatorSpline::PosArray::zeroPosArray()
        {
            static const PosArray pa = []
            {
                PosArray p;
                p.initToZero();
                return p;
            }();
            return pa;
        }
    } // ns
} // ns