#include "LocationParser.h"

#include <QRegularExpression>

#include <cmath>

namespace {

double toDouble(QString value, bool *ok)
{
    value.replace(',', '.');
    return value.toDouble(ok);
}

QString normalizedCoordinateText(QString value)
{
    value.replace(',', '.');
    if (value.startsWith('+')) {
        value.remove(0, 1);
    }
    return value;
}

QString normalizeKey(QString key)
{
    key = key.toLower();
    key.remove('_');
    key.remove('-');
    key.remove(' ');

    if (key == "lat" || key == "latitude" || key == "lattitude") {
        return "latitude";
    }

    if (key == "long" || key == "longitude" || key == "lon" || key == "lng") {
        return "longitude";
    }

    if (key == "hheight" || key == "height" || key == "alt" || key == "altitude") {
        return "hheight";
    }

    if (key == "sat" || key == "sats" || key == "satellite" || key == "satellites"
        || key == "activesat" || key == "activesats" || key == "activesatellite"
        || key == "activesatellites" || key == "numsat" || key == "numsats"
        || key == "numbersat" || key == "numbersats" || key == "satactive"
        || key == "satsactive" || key == "satelliteactive" || key == "satellitesactive"
        || key == "uydu" || key == "uydular") {
        return "satellites";
    }

    if (key == "tracked") {
        return "satellites";
    }

    if (key == "gdop" || key == "pdop" || key == "hdop" || key == "vdop" || key == "tdop") {
        return key;
    }

    return {};
}

bool isValidLatitude(double latitude)
{
    return std::isfinite(latitude) && latitude >= -90.0 && latitude <= 90.0;
}

bool isValidLongitude(double longitude)
{
    return std::isfinite(longitude) && longitude >= -180.0 && longitude <= 180.0;
}

} // namespace

std::optional<ParsedTelemetry> LocationParser::parseLine(const QString &line)
{
    const QString trimmed = line.trimmed();
    if (trimmed.isEmpty()) {
        return std::nullopt;
    }

    static const QRegularExpression labeledValueRegex(
        QStringLiteral(R"((^|[^A-Za-z0-9_])(?<key>lat(?:t?itude)?|long(?:itude)?|lon|lng|hheight|height|alt(?:itude)?|active[_ -]?sat(?:ellite)?s?|num(?:ber)?[_ -]?sat(?:ellite)?s?|sat(?:ellite)?s?[_ -]?active|sat(?:ellite)?s?|sats|tracked|uydu(?:lar)?|[gphvt][_ -]?dop)(?:\s*(?:=|:|->)?\s*)(?<value>[+-]?(?:\d+(?:[\.,]\d+)?|[\.,]\d+)(?:[eE][+-]?\d+)?))"),
        QRegularExpression::CaseInsensitiveOption);
    static const QRegularExpression satelliteCountPhraseRegex(
        QStringLiteral(R"((^|[^A-Za-z0-9_\.])(?<value>\d+)\s+(?:active\s+)?(?:sat(?:ellite)?s?|uydu(?:lar)?)(?=$|[\s,.;)\]]))"),
        QRegularExpression::CaseInsensitiveOption);

    ParsedTelemetry telemetry;
    telemetry.rawLine = trimmed;
    static const QRegularExpression sourceRegex(
        QStringLiteral(R"((?:^|\s)source\s*=\s*(?<source>gps|imu[_-]?corrected|trajectory|ground[_-]?truth)(?=$|\s))"),
        QRegularExpression::CaseInsensitiveOption);
    const QRegularExpressionMatch sourceMatch = sourceRegex.match(trimmed);
    if (sourceMatch.hasMatch()) {
        const QString source = sourceMatch.captured(QStringLiteral("source"));
        if (source.startsWith(QStringLiteral("imu"), Qt::CaseInsensitive)) {
            telemetry.source = LocationSource::ImuCorrected;
        } else if (source.startsWith(QStringLiteral("trajectory"), Qt::CaseInsensitive)
                   || source.startsWith(QStringLiteral("ground"), Qt::CaseInsensitive)) {
            telemetry.source = LocationSource::Trajectory;
        } else {
            telemetry.source = LocationSource::Gps;
        }
    }
    static const QRegularExpression fusionModeRegex(
        QStringLiteral(R"((?:^|\s)mode\s*=\s*(?<mode>gps[_-]?aided|dead[_-]?reckoning)(?=$|\s))"),
        QRegularExpression::CaseInsensitiveOption);
    const QRegularExpressionMatch fusionModeMatch = fusionModeRegex.match(trimmed);
    if (fusionModeMatch.hasMatch()) {
        telemetry.fusionMode = fusionModeMatch.captured(QStringLiteral("mode")).startsWith(
                                   QStringLiteral("dead"), Qt::CaseInsensitive)
            ? FusionMode::DeadReckoning : FusionMode::GpsAided;
    }
    static const QRegularExpression gpsAgeRegex(
        QStringLiteral(R"((?:^|\s)gps[_-]?age[_-]?ms\s*=\s*(?<age>\d+)(?=$|\s))"),
        QRegularExpression::CaseInsensitiveOption);
    const QRegularExpressionMatch gpsAgeMatch = gpsAgeRegex.match(trimmed);
    if (gpsAgeMatch.hasMatch()) {
        bool ok = false;
        const qint64 ageMs = gpsAgeMatch.captured(QStringLiteral("age")).toLongLong(&ok);
        if (ok) {
            telemetry.gpsAgeMs = ageMs;
        }
    }
    bool hadCoordinateField = false;
    bool hadLatitudeField = false;
    bool hadLongitudeField = false;

    auto matches = labeledValueRegex.globalMatch(trimmed);
    while (matches.hasNext()) {
        const QRegularExpressionMatch match = matches.next();
        const QString field = normalizeKey(match.captured(QStringLiteral("key")));
        if (field.isEmpty()) {
            continue;
        }

        bool ok = false;
        const double value = toDouble(match.captured(QStringLiteral("value")), &ok);
        if (!ok || !std::isfinite(value)) {
            continue;
        }

        if (field == "latitude") {
            hadCoordinateField = true;
            hadLatitudeField = true;
            telemetry.latitude = value;
            telemetry.latitudeText = normalizedCoordinateText(
                match.captured(QStringLiteral("value")));
        } else if (field == "longitude") {
            hadCoordinateField = true;
            hadLongitudeField = true;
            telemetry.longitude = value;
            telemetry.longitudeText = normalizedCoordinateText(
                match.captured(QStringLiteral("value")));
        } else if (field == "hheight") {
            telemetry.hheight = value;
        } else if (field == "satellites") {
            const int count = static_cast<int>(std::llround(value));
            if (count >= 0) {
                telemetry.satellites = count;
            }
        } else if (field == "gdop") {
            if (value >= 0.0) {
                telemetry.dop.gdop = value;
            }
        } else if (field == "pdop") {
            if (value >= 0.0) {
                telemetry.dop.pdop = value;
            }
        } else if (field == "hdop") {
            if (value >= 0.0) {
                telemetry.dop.hdop = value;
            }
        } else if (field == "vdop") {
            if (value >= 0.0) {
                telemetry.dop.vdop = value;
            }
        } else if (field == "tdop") {
            if (value >= 0.0) {
                telemetry.dop.tdop = value;
            }
        }
    }

    auto satellitePhraseMatches = satelliteCountPhraseRegex.globalMatch(trimmed);
    while (satellitePhraseMatches.hasNext()) {
        const QRegularExpressionMatch match = satellitePhraseMatches.next();
        bool ok = false;
        const int count = match.captured(QStringLiteral("value")).toInt(&ok);
        if (ok && count >= 0) {
            telemetry.satellites = count;
        }
    }

    if (telemetry.latitude.has_value() && !isValidLatitude(*telemetry.latitude)) {
        telemetry.latitude.reset();
        telemetry.latitudeText.clear();
    }
    if (telemetry.longitude.has_value() && !isValidLongitude(*telemetry.longitude)) {
        telemetry.longitude.reset();
        telemetry.longitudeText.clear();
    }

    if (hadLatitudeField && hadLongitudeField
        && (!telemetry.hasLocation()
            || !isValidCoordinate(*telemetry.latitude, *telemetry.longitude))) {
        telemetry.latitude.reset();
        telemetry.longitude.reset();
        telemetry.latitudeText.clear();
        telemetry.longitudeText.clear();
        telemetry.hheight.reset();
    } else if (hadCoordinateField && !telemetry.latitude.has_value()
               && !telemetry.longitude.has_value()) {
        telemetry.hheight.reset();
    }

    if (!telemetry.hasAnyValue()) {
        return std::nullopt;
    }

    return telemetry;
}

bool LocationParser::isValidCoordinate(double latitude, double longitude)
{
    return std::isfinite(latitude) && std::isfinite(longitude)
        && latitude >= -90.0 && latitude <= 90.0
        && longitude >= -180.0 && longitude <= 180.0;
}

QSet<QString> LocationParser::parseGpsPrns(const QString &line)
{
    QSet<QString> result;
    const auto addPrn = [&result](QString value, bool zeroBased = false) {
        value = value.trimmed().toUpper();
        if (value.startsWith(QLatin1Char('G'))) value.remove(0, 1);
        bool ok = false;
        int number = value.toInt(&ok);
        if (ok && zeroBased) ++number;
        if (ok && number >= 1 && number <= 63) {
            result.insert(QStringLiteral("G%1").arg(number, 2, 10, QLatin1Char('0')));
        }
    };

    static const QRegularExpression gpsToken(
        QStringLiteral(R"((?:^|[^A-Za-z0-9])G(?<prn>\d{1,2})(?=$|[^0-9]))"),
        QRegularExpression::CaseInsensitiveOption);
    auto gpsMatches = gpsToken.globalMatch(line);
    while (gpsMatches.hasNext()) addPrn(gpsMatches.next().captured(QStringLiteral("prn")));

    static const QRegularExpression labeledList(
        QStringLiteral(R"((?<label>\bPRNS?\b|\bSVS?\b|\bSVNS?\b|\bSVIDS?\b)\s*(?:=|:|->)?\s*[\[(]?\s*(?<list>(?:G?\d{1,2})(?:\s*[,;/| ]\s*G?\d{1,2})*)\s*[\])]?)"),
        QRegularExpression::CaseInsensitiveOption);
    static const QRegularExpression number(QStringLiteral(R"(G?(\d{1,2}))"),
                                            QRegularExpression::CaseInsensitiveOption);
    auto listMatches = labeledList.globalMatch(line);
    while (listMatches.hasNext()) {
        const QRegularExpressionMatch listMatch = listMatches.next();
        const bool zeroBased = listMatch.captured(QStringLiteral("label")).startsWith(
            QStringLiteral("SVN"), Qt::CaseInsensitive);
        auto values = number.globalMatch(listMatch.captured(QStringLiteral("list")));
        while (values.hasNext()) addPrn(values.next().captured(1), zeroBased);
    }
    return result;
}