#include "../src/LocationParser.h"
#include "../src/RinexNavigation.h"

#include <QCoreApplication>
#include <QFile>

#include <cassert>
#include <cmath>

namespace {

bool closeTo(double left, double right, double tolerance = 0.000001)
{
    return std::fabs(left - right) < tolerance;
}

QString rinexField(double value)
{
    return QStringLiteral("%1").arg(value, 19, 'E', 12);
}

QString rinexContinuation(std::initializer_list<double> values)
{
    QString line(4, QLatin1Char(' '));
    for (double value : values) line += rinexField(value);
    return line + QLatin1Char('\n');
}

QByteArray syntheticGpsNavigation(const QDateTime &gpsEpochLabel, int gpsWeek, double toe)
{
    QString rinex = QStringLiteral(
        "     3.05           N: GNSS NAV DATA    M: MIXED            RINEX VERSION / TYPE\n"
        "                                                            END OF HEADER\n");
    rinex += QStringLiteral("G01 %1").arg(
        gpsEpochLabel.toString(QStringLiteral("yyyy MM dd HH mm ss")));
    rinex += rinexField(0.0) + rinexField(0.0) + rinexField(0.0) + QLatin1Char('\n');
    rinex += rinexContinuation({1.0, 0.0, 0.0, 0.0});
    rinex += rinexContinuation({0.0, 0.0, 0.0, 5153.7954775});
    rinex += rinexContinuation({toe, 0.0, 0.0, 0.0});
    rinex += rinexContinuation({0.0, 0.0, 0.0, 0.0});
    rinex += rinexContinuation({0.0, 0.0, static_cast<double>(gpsWeek), 0.0});
    rinex += rinexContinuation({2.0, 0.0, 0.0, 1.0});
    rinex += rinexContinuation({0.0, 0.0, 0.0, 0.0});
    return rinex.toLatin1();
}

} // namespace

int main(int argc, char **argv)
{
    QCoreApplication app(argc, argv);

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("DBG lat=41.0082000 long=28.9784000 hheight=123.45 sats=7"));
        assert(parsed.has_value());
        assert(parsed->hasLocation());
        assert(closeTo(*parsed->latitude, 41.0082));
        assert(closeTo(*parsed->longitude, 28.9784));
        assert(closeTo(*parsed->hheight, 123.45));
        assert(parsed->satellites == 7);
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("latitude: -12,5 longitude: 33,25 height: 99 activeSats: 4"));
        assert(parsed.has_value());
        assert(parsed->hasLocation());
        assert(closeTo(*parsed->latitude, -12.5));
        assert(closeTo(*parsed->longitude, 33.25));
        assert(parsed->latitudeText == QStringLiteral("-12.5"));
        assert(parsed->longitudeText == QStringLiteral("33.25"));
        assert(closeTo(*parsed->hheight, 99.0));
        assert(parsed->satellites == 4);
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral("satellites_active=11"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(parsed->satellites == 11);
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("lat=200 long=28 hheight=100 active_sats=8"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(parsed->latitudeText.isEmpty());
        assert(parsed->longitudeText.isEmpty());
        assert(!parsed->hheight.has_value());
        assert(parsed->satellites == 8);
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("lattitude : 40.78925034214206846173"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(closeTo(*parsed->latitude, 40.78925034214206846173));
        assert(parsed->latitudeText == QStringLiteral("40.78925034214206846173"));
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("longitude : 29.46277530841727809729"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(closeTo(*parsed->longitude, 29.46277530841727809729));
        assert(parsed->longitudeText == QStringLiteral("29.46277530841727809729"));
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("height    : -89.05779508221894502640 "));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(closeTo(*parsed->hheight, -89.05779508221894502640));
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("NAV solve start sats:4 mode:periodic abs_span_ms:2.176"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(parsed->satellites == 4);
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("[NAV] navigation software running with 5 satellites"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(parsed->satellites == 5);
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral("GDOP : 3.9576744550"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(closeTo(*parsed->dop.gdop, 3.9576744550));
    }

    {
        const auto parsed = LocationParser::parseLine(
            QStringLiteral("PDOP : 3.2540598660 HDOP : 2.3631161789 VDOP : 2.2370935468 TDOP : 2.2526165852"));
        assert(parsed.has_value());
        assert(!parsed->hasLocation());
        assert(closeTo(*parsed->dop.pdop, 3.2540598660));
        assert(closeTo(*parsed->dop.hdop, 2.3631161789));
        assert(closeTo(*parsed->dop.vdop, 2.2370935468));
        assert(closeTo(*parsed->dop.tdop, 2.2526165852));
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral("plain debug noise"));
        assert(!parsed.has_value());
    }

    {
        const QSet<QString> prns = LocationParser::parseGpsPrns(
            QStringLiteral("tracking PRNs: 3, G08, 14; SV=G21 SVID: 27"));
        assert(prns.contains(QStringLiteral("G03")));
        assert(prns.contains(QStringLiteral("G08")));
        assert(prns.contains(QStringLiteral("G14")));
        assert(prns.contains(QStringLiteral("G21")));
        assert(prns.contains(QStringLiteral("G27")));
        assert(prns.size() == 5);
    }

    {
        const QString line = QStringLiteral(
            "[OK] [TRK] tracked satellites=5 svn=[2,3,5,8,30]");
        const auto parsed = LocationParser::parseLine(line);
        assert(parsed.has_value());
        assert(parsed->satellites == 5);
        const QSet<QString> prns = LocationParser::parseGpsPrns(line);
        assert(prns == QSet<QString>({QStringLiteral("G03"), QStringLiteral("G04"),
                                     QStringLiteral("G06"), QStringLiteral("G09"),
                                     QStringLiteral("G31")}));
    }

    {
        const QSet<QString> prns = LocationParser::parseGpsPrns(
            QStringLiteral("[TRK] tracked satellites=2 svn=[0,31]"));
        assert(prns == QSet<QString>({QStringLiteral("G01"), QStringLiteral("G32")}));
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral(
            "[OK] [NAV] DOP GDOP=7.0907891622 PDOP=5.6333862175 "
            "HDOP=2.9819528958 VDOP=4.7794348204 TDOP=4.3063035967"));
        assert(parsed.has_value());
        assert(closeTo(*parsed->dop.gdop, 7.0907891622));
        assert(closeTo(*parsed->dop.pdop, 5.6333862175));
        assert(closeTo(*parsed->dop.hdop, 2.9819528958));
        assert(closeTo(*parsed->dop.vdop, 4.7794348204));
        assert(closeTo(*parsed->dop.tdop, 4.3063035967));
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral(
            "[OK] [NAV] position lat=40.77831900791127850425 "
            "lon=29.43579105837637044374 height=198.80186179187148809433"));
        assert(parsed.has_value());
        assert(parsed->hasLocation());
        assert(closeTo(*parsed->latitude, 40.77831900791127850425));
        assert(closeTo(*parsed->longitude, 29.43579105837637044374));
        assert(closeTo(*parsed->hheight, 198.80186179187148809433));
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral(
            "[LOCATION] source=imu_corrected mode=dead_reckoning "
            "lat=40.7356 lon=31.5985 height=812.7 gps_age_ms=3180 "
            "gdop=4.7 pdop=4.1 hdop=2.7 vdop=3.0 tdop=2.3 tracked=6 used=5"));
        assert(parsed.has_value());
        assert(parsed->hasLocation());
        assert(parsed->source == LocationSource::ImuCorrected);
        assert(parsed->fusionMode == FusionMode::DeadReckoning);
        assert(parsed->gpsAgeMs == 3180);
        assert(parsed->satellites == 6);
        assert(closeTo(*parsed->dop.hdop, 2.7));
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral(
            "[LOCATION] source=gps lat=40.7357 lon=31.5986 height=811 tracked=5 used=5"));
        assert(parsed.has_value());
        assert(parsed->source == LocationSource::Gps);
        assert(parsed->satellites == 5);
    }

    {
        const auto parsed = LocationParser::parseLine(QStringLiteral(
            "[LOCATION] source=trajectory lat=40.7358 lon=31.5987 height=810 tracked=4 used=4"));
        assert(parsed.has_value());
        assert(parsed->hasLocation());
        assert(parsed->source == LocationSource::Trajectory);
        assert(parsed->satellites == 4);
    }

    {
        constexpr int gpsWeek = 2428;
        constexpr double toe = 403200.0;
        const QDateTime gpsEpoch(QDate(1980, 1, 6), QTime(0, 0), Qt::UTC);
        const QDateTime gpsLabel = gpsEpoch.addSecs(
            static_cast<qint64>(gpsWeek * 604800.0 + toe));
        const QDateTime matchingUtc = gpsLabel.addSecs(-18);
        QString error;
        const auto satellites = RinexNavigation::parseGpsNavigation(
            syntheticGpsNavigation(gpsLabel, gpsWeek, toe), matchingUtc, &error);
        assert(error.isEmpty());
        assert(satellites.size() == 1);
        const double semiMajorAxis = 5153.7954775 * 5153.7954775;
        const double node = -7.2921151467e-5 * toe;
        assert(closeTo(satellites[0].x, semiMajorAxis * std::cos(node), 0.1));
        assert(closeTo(satellites[0].y, semiMajorAxis * std::sin(node), 0.1));
        assert(closeTo(satellites[0].z, 0.0, 0.1));
    }

    const QString rinexPath = qEnvironmentVariable("LOCATION_PLOTTER_TEST_RINEX");
    if (!rinexPath.isEmpty()) {
        QFile file(rinexPath);
        assert(file.open(QIODevice::ReadOnly));
        QString error;
        const auto satellites = RinexNavigation::parseGpsNavigation(
            file.readAll(), QDateTime::currentDateTimeUtc(), &error);
        assert(satellites.size() >= 20);
        for (const auto &satellite : satellites) {
            assert(satellite.prn.startsWith(QStringLiteral("G")));
            assert(std::isfinite(satellite.x));
            assert(std::isfinite(satellite.y));
            assert(std::isfinite(satellite.z));
        }
    }

    return 0;
}