#include "nmea_parser.h"

#include <cstdlib>

namespace roro::gnss {

namespace {

constexpr double kKmhPerKnot = 1.852;

int hexValue(char c) {
    if (c >= '0' && c <= '9') return c - '0';
    if (c >= 'A' && c <= 'F') return c - 'A' + 10;
    if (c >= 'a' && c <= 'f') return c - 'a' + 10;
    return -1;
}

Constellation fromTalker(const std::string& t) {
    if (t == "GP") return Constellation::Gps;
    if (t == "GL") return Constellation::Glonass;
    if (t == "GA") return Constellation::Galileo;
    if (t == "GB" || t == "BD") return Constellation::BeiDou;
    if (t == "GQ" || t == "QZ") return Constellation::Qzss;
    if (t == "GI") return Constellation::Navic;
    return Constellation::Unknown;  // GN: combined
}

// GSA's system ID field (NMEA 4.10).
Constellation fromSystemId(int id) {
    switch (id) {
        case 1: return Constellation::Gps;
        case 2: return Constellation::Glonass;
        case 3: return Constellation::Galileo;
        case 4: return Constellation::BeiDou;
        case 5: return Constellation::Qzss;
        case 6: return Constellation::Navic;
        default: return Constellation::Unknown;
    }
}

bool number(const std::string& s, double& out) {
    if (s.empty()) return false;
    char* end;
    out = std::strtod(s.c_str(), &end);
    return *end == '\0';
}

int integer(const std::string& s, int fallback = 0) {
    double v;
    return number(s, v) ? static_cast<int>(v) : fallback;
}

// "ddmm.mmmm" / "dddmm.mmmm" with its hemisphere letter.
bool coordinate(const std::string& value, const std::string& hemisphere, double& out) {
    double raw;
    if (!number(value, raw) || hemisphere.empty()) return false;
    int degrees = static_cast<int>(raw / 100);
    out = degrees + (raw - degrees * 100) / 60.0;
    if (hemisphere == "S" || hemisphere == "W") out = -out;
    return true;
}

// Days since 1970-01-01 for a civil date (Howard Hinnant's algorithm).
int64_t daysFromCivil(int y, int m, int d) {
    y -= m <= 2;
    const int64_t era = (y >= 0 ? y : y - 399) / 400;
    const unsigned yoe = static_cast<unsigned>(y - era * 400);
    const unsigned doy = (153 * (m + (m > 2 ? -3 : 9)) + 2) / 5 + d - 1;
    const unsigned doe = yoe * 365 + yoe / 4 - yoe / 100 + doy;
    return era * 146097 + static_cast<int64_t>(doe) - 719468;
}

}  // namespace

const char* constellationName(Constellation c) {
    switch (c) {
        case Constellation::Gps: return "GPS";
        case Constellation::Glonass: return "GLONASS";
        case Constellation::Galileo: return "Galileo";
        case Constellation::BeiDou: return "BeiDou";
        case Constellation::Qzss: return "QZSS";
        case Constellation::Navic: return "NavIC";
        default: return "?";
    }
}

int64_t GnssState::utcSeconds() const {
    return daysFromCivil(year, month, day) * 86400 + hour * 3600 + minute * 60 + second;
}

void NmeaParser::feed(const char* data, size_t len) {
    for (size_t i = 0; i < len; i++) {
        char c = data[i];
        if (c == '\n') {
            if (!overflow_ && !line_.empty()) {
                if (onLine) onLine(line_);
                sentence(line_);
            }
            line_.clear();
            overflow_ = false;
        } else if (c != '\r') {
            if (line_.size() >= kMaxLine) overflow_ = true;
            else line_ += c;
        }
    }
}

bool NmeaParser::sentence(const std::string& line) {
    size_t star = line.rfind('*');
    if (line.size() < 7 || line[0] != '$' || star == std::string::npos || star + 3 != line.size()) {
        bad_++;
        return false;
    }
    uint8_t sum = 0;
    for (size_t i = 1; i < star; i++) sum ^= static_cast<uint8_t>(line[i]);
    int hi = hexValue(line[star + 1]), lo = hexValue(line[star + 2]);
    if (hi < 0 || lo < 0 || sum != (hi << 4 | lo)) {
        bad_++;
        return false;
    }
    good_++;

    Fields f;
    size_t start = 1;
    for (size_t i = 1; i <= star; i++)
        if (i == star || line[i] == ',') {
            f.push_back(line.substr(start, i - start));
            start = i + 1;
        }
    if (f[0].size() < 5) return true;
    std::string talker = f[0].substr(0, 2), type = f[0].substr(f[0].size() - 3);
    if (type == "RMC") rmc(f);
    else if (type == "GGA") gga(f);
    else if (type == "GSA") gsa(f, fromTalker(talker));
    else if (type == "GSV") gsv(f, fromTalker(talker));
    return true;
}

void NmeaParser::rmc(const Fields& f) {
    if (f.size() < 10) return;
    bool active = f[2] == "A";
    double lat, lon;
    state_.positionValid = active && coordinate(f[3], f[4], lat) && coordinate(f[5], f[6], lon);
    if (state_.positionValid) {
        state_.latitude = lat;
        state_.longitude = lon;
    }
    double knots, course;
    state_.speedKmh = active && number(f[7], knots) ? static_cast<float>(knots * kKmhPerKnot) : 0;
    state_.courseValid = active && number(f[8], course);
    if (state_.courseValid) state_.courseDeg = static_cast<float>(course);

    const std::string &t = f[1], &d = f[9];
    state_.timeValid = active && t.size() >= 6 && d.size() == 6;
    if (state_.timeValid) {
        state_.hour = integer(t.substr(0, 2));
        state_.minute = integer(t.substr(2, 2));
        state_.second = integer(t.substr(4, 2));
        state_.day = integer(d.substr(0, 2));
        state_.month = integer(d.substr(2, 2));
        state_.year = 2000 + integer(d.substr(4, 2));
    }
}

void NmeaParser::gga(const Fields& f) {
    if (f.size() < 10) return;
    int quality = integer(f[6]);
    double lat, lon, hdop, alt;
    state_.positionValid = quality > 0 && coordinate(f[2], f[3], lat) && coordinate(f[4], f[5], lon);
    if (state_.positionValid) {
        state_.latitude = lat;
        state_.longitude = lon;
    }
    state_.satellitesUsed = integer(f[7]);
    if (number(f[8], hdop)) state_.hdop = static_cast<float>(hdop);
    state_.altitudeValid = quality > 0 && number(f[9], alt);
    if (state_.altitudeValid) state_.altitudeM = static_cast<float>(alt);
}

void NmeaParser::gsa(const Fields& f, Constellation talker) {
    if (f.size() < 18) return;
    int mode = integer(f[2], 1);
    state_.fix = mode == 3 ? FixType::ThreeD : mode == 2 ? FixType::TwoD : FixType::None;
    Constellation system = f.size() > 18 ? fromSystemId(integer(f[18])) : talker;
    if (system == Constellation::Unknown) return;  // can't tell whose satellites these are
    std::set<int>& used = used_[static_cast<int>(system)];
    used.clear();
    for (size_t i = 3; i <= 14; i++)
        if (!f[i].empty()) used.insert(integer(f[i]));
    rebuildSatellites();
}

void NmeaParser::gsv(const Fields& f, Constellation talker) {
    if (f.size() < 4 || talker == Constellation::Unknown) return;
    int total = integer(f[1]), number = integer(f[2]);
    size_t extra = f.size() - 4;
    int signal = extra % 4 == 1 ? integer(f.back()) : 0;  // NMEA 4.10 signal ID, last
    auto key = std::make_pair(static_cast<int>(talker), signal);
    std::vector<Satellite>& pending = pending_[key];
    if (number == 1) pending.clear();
    for (size_t i = 4; i + 3 < f.size(); i += 4) {
        Satellite s;
        s.system = talker;
        s.prn = integer(f[i]);
        s.elevation = integer(f[i + 1]);
        s.azimuth = integer(f[i + 2]);
        s.snr = integer(f[i + 3], -1);
        if (s.prn > 0) pending.push_back(s);
    }
    if (number == total) {
        inView_[key] = pending;
        pending.clear();
        rebuildSatellites();
    }
}

void NmeaParser::rebuildSatellites() {
    std::vector<Satellite> merged;
    for (auto& [key, list] : inView_)
        for (const Satellite& s : list) {
            Satellite* same = nullptr;
            for (auto& m : merged)
                if (m.system == s.system && m.prn == s.prn) same = &m;
            if (!same) merged.push_back(s);
            else if (s.snr > same->snr) same->snr = s.snr;
        }
    for (auto& s : merged) {
        auto it = used_.find(static_cast<int>(s.system));
        s.used = it != used_.end() && it->second.count(s.prn) > 0;
    }
    state_.satellites = std::move(merged);
}

}  // namespace roro::gnss