#include "GnssManager.h"

#include <ctype.h>
#include <math.h>
#include <stdarg.h>
#include <string.h>
#include "Config.h"

namespace field_qa {

namespace {

enum class GpsModuleKind : uint8_t {
  Unknown = 0,
  L76K,
  Ublox
};

#if defined(GPS_UBLOX)
static constexpr GpsModuleKind kExpectedGpsModule = GpsModuleKind::Ublox;
#elif defined(GPS_L76K)
static constexpr GpsModuleKind kExpectedGpsModule = GpsModuleKind::L76K;
#else
static constexpr GpsModuleKind kExpectedGpsModule = GpsModuleKind::Unknown;
#endif

static GpsModuleKind talkerToConstellation(const char* talker) {
  if (!talker) return GpsModuleKind::Unknown;
  if (strcmp(talker, "GP") == 0) return GpsModuleKind::L76K;
  if (strcmp(talker, "GA") == 0) return GpsModuleKind::Ublox;
  return GpsModuleKind::Unknown;
}

static FixType fixTypeFromQuality(int quality, int dimension) {
  switch (quality) {
    case 2:
      return FixType::Dgps;
    case 4:
      return FixType::RtkFixed;
    case 5:
      return FixType::RtkFloat;
    default:
      if (dimension >= 3) return FixType::Fix3D;
      if (dimension == 2) return FixType::Fix2D;
      return FixType::NoFix;
  }
}

static void copyTalker(const char* header, char* out) {
  if (!header || strlen(header) < 3) {
    out[0] = '?';
    out[1] = '?';
    out[2] = '\0';
    return;
  }
  out[0] = header[1];
  out[1] = header[2];
  out[2] = '\0';
}

static void gnssDiagf(const char* fmt, ...) {
#if GNSS_DIAG
  char buf[192];
  va_list ap;
  va_start(ap, fmt);
  vsnprintf(buf, sizeof(buf), fmt, ap);
  va_end(ap);
  Serial.print("[GNSS] ");
  Serial.println(buf);
#else
  (void)fmt;
#endif
}

}  // namespace

void GnssManager::begin() {
  m_bootMs = millis();
  strlcpy(m_detectedChip, kGnssChip, sizeof(m_detectedChip));
#ifdef GPS_1PPS_PIN
  pinMode(GPS_1PPS_PIN, INPUT);
#endif
#ifdef GPS_WAKEUP_PIN
  pinMode(GPS_WAKEUP_PIN, INPUT);
#endif
  startUart(GPS_BAUD, GPS_RX_PIN, GPS_TX_PIN);
}

void GnssManager::startUart(uint32_t baud, int rxPin, int txPin) {
  m_serial.end();
  delay(20);
  m_serial.setRxBufferSize(2048);
  m_serial.begin(baud, SERIAL_8N1, rxPin, txPin);
}

bool GnssManager::collectTraffic(uint32_t windowMs) {
  uint32_t start = millis();
  bool sawBytes = false;
  while ((uint32_t)(millis() - start) < windowMs) {
    if (m_serial.available() > 0) {
      sawBytes = true;
    }
    poll();
    delay(2);
  }
  return sawBytes || m_sawSentence;
}

bool GnssManager::probeAtBaud(uint32_t baud, int rxPin, int txPin) {
  startUart(baud, rxPin, txPin);
  if (collectTraffic(700)) {
    return true;
  }
  m_serial.write("$PCAS06,0*1B\r\n");
  m_serial.write("$PMTK605*31\r\n");
  m_serial.write("$PQTMVERNO*58\r\n");
  m_serial.write("$PUBX,00*33\r\n");
  m_serial.write("$PMTK353,1,1,1,1,1*2A\r\n");
  m_serial.write("$PMTK314,0,1,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");
  return collectTraffic(1200);
}

bool GnssManager::probeAtStartup(Stream& serialOut) {
  const uint32_t bauds[] = {GPS_BAUD, 115200, 38400, 57600, 19200};
  int pins[2][2] = {{GPS_RX_PIN, GPS_TX_PIN}, {34, 12}};
  size_t pinCount = (kExpectedGpsModule == GpsModuleKind::Ublox && !(GPS_RX_PIN == 34 && GPS_TX_PIN == 12)) ? 2 : 1;
  for (size_t p = 0; p < pinCount; ++p) {
    for (size_t i = 0; i < sizeof(bauds) / sizeof(bauds[0]); ++i) {
      if (probeAtBaud(bauds[i], pins[p][0], pins[p][1])) {
        serialOut.printf("GPS traffic detected at baud=%lu rx=%d tx=%d\n",
                         (unsigned long)bauds[i], pins[p][0], pins[p][1]);
        return true;
      }
    }
  }
  serialOut.println("WARNING: no GPS traffic detected during startup probe");
  return false;
}

bool GnssManager::parseUInt2(const char* s, uint8_t& out) {
  if (!s || !isdigit((unsigned char)s[0]) || !isdigit((unsigned char)s[1])) {
    return false;
  }
  out = (uint8_t)((s[0] - '0') * 10 + (s[1] - '0'));
  return true;
}

double GnssManager::parseNmeaCoord(const char* value, const char* hemi) {
  if (!value || !value[0] || !hemi || !hemi[0]) {
    return 0.0;
  }
  double raw = atof(value);
  double deg = floor(raw / 100.0);
  double minutes = raw - (deg * 100.0);
  double result = deg + minutes / 60.0;
  if (hemi[0] == 'S' || hemi[0] == 'W') {
    result = -result;
  }
  return result;
}

int GnssManager::splitCsvPreserveEmpty(char* line, char* fields[], int maxFields) {
  if (!line || !fields || maxFields <= 0) {
    return 0;
  }
  int count = 0;
  char* p = line;
  fields[count++] = p;
  while (*p && count < maxFields) {
    if (*p == ',') {
      *p = '\0';
      fields[count++] = p + 1;
    }
    ++p;
  }
  return count;
}

void GnssManager::parseGga(char* fields[], int count) {
  if (count < 10) {
    return;
  }
  const int quality = atoi(fields[6]);
  const int satsUsed = atoi(fields[7]);
  if (satsUsed >= 0) {
    m_state.satsUsed = satsUsed;
  }
  if (fields[8] && fields[8][0]) {
    m_state.hdop = atof(fields[8]);
    m_state.validHdop = true;
  }
  if (fields[9] && fields[9][0]) {
    m_state.altitudeM = atof(fields[9]);
    m_state.validAltitude = true;
  }
  if (fields[2] && fields[2][0] && fields[4] && fields[4][0]) {
    m_state.latitude = parseNmeaCoord(fields[2], fields[3]);
    m_state.longitude = parseNmeaCoord(fields[4], fields[5]);
    m_state.validLocation = true;
  }
  if (quality > 0) {
    m_state.validFix = true;
    m_lastFixMs = millis();
  } else {
    m_state.validFix = false;
  }
  m_state.fixType = fixTypeFromQuality(quality, m_state.fixDimension);
}

void GnssManager::parseGsa(char* fields[], int count) {
  if (count < 18) {
    return;
  }
  const uint32_t now = millis();
  if ((uint32_t)(now - m_lastGsaMs) > 1500U) {
    m_usedPrnCount = 0;
    memset(m_usedPrns, 0, sizeof(m_usedPrns));
  }
  m_lastGsaMs = now;
  gnssDiagf("GSA %s", fields[0] ? fields[0] : "?");
  const int dim = atoi(fields[2]);
  m_state.fixDimension = dim;
  if (count > 15 && fields[15] && fields[15][0]) {
    m_state.pdop = atof(fields[15]);
    m_state.validPdop = true;
  }
  if (count > 16 && fields[16] && fields[16][0]) {
    m_state.hdop = atof(fields[16]);
    m_state.validHdop = true;
  }
  if (count > 17 && fields[17] && fields[17][0]) {
    m_state.vdop = atof(fields[17]);
    m_state.validVdop = true;
  }

  int satsUsed = 0;
  for (int i = 3; i <= 14 && i < count; ++i) {
    if (fields[i] && fields[i][0]) {
      ++satsUsed;
      addUsedPrn((uint8_t)atoi(fields[i]));
    }
  }
  if (satsUsed > 0) {
    m_state.satsUsed = satsUsed;
  }
  if (dim >= 2) {
    m_state.validFix = true;
    m_lastFixMs = millis();
  }
  m_state.fixType = fixTypeFromQuality(m_state.validFix ? 1 : 0, dim);
}

void GnssManager::clearSatelliteView() {
  m_satCount = 0;
  for (size_t i = 0; i < kMaxSatellites; ++i) {
    m_satellites[i] = SatelliteInfo{};
  }
  m_state.gpsCount = 0;
  m_state.galileoCount = 0;
  m_state.glonassCount = 0;
  m_state.beidouCount = 0;
  m_state.navicCount = 0;
  m_state.qzssCount = 0;
  m_state.sbasCount = 0;
  m_state.meanSnr = -1.0f;
  m_state.maxSnr = 0;
}

void GnssManager::clearTalkerSatellites(const char* talker) {
  if (!talker || !talker[0]) {
    return;
  }

  size_t write = 0;
  size_t removed = 0;
  for (size_t read = 0; read < m_satCount; ++read) {
    SatelliteInfo sat = m_satellites[read];
    if (sat.valid && strcmp(sat.talker, talker) == 0) {
      ++removed;
      continue;
    }
    if (write != read) {
      m_satellites[write] = sat;
    }
    ++write;
  }
  for (size_t i = write; i < m_satCount; ++i) {
    m_satellites[i] = SatelliteInfo{};
  }
  if (removed > 0) {
    gnssDiagf("clear talker=%s removed=%u remaining=%u",
              talker,
              (unsigned)removed,
              (unsigned)write);
  }
  m_satCount = write;
}

void GnssManager::finalizeSatelliteStats() {
  uint32_t snrSum = 0;
  uint32_t snrCount = 0;
  for (size_t i = 0; i < m_satCount; ++i) {
    SatelliteInfo& sat = m_satellites[i];
    if (!sat.valid) {
      continue;
    }
    sat.usedInSolution = prnUsedInSolution(sat.prn);
    if (strcmp(sat.talker, "GP") == 0 || strcmp(sat.talker, "GN") == 0) {
      ++m_state.gpsCount;
    } else if (strcmp(sat.talker, "GA") == 0) {
      ++m_state.galileoCount;
    } else if (strcmp(sat.talker, "GL") == 0) {
      ++m_state.glonassCount;
    } else if (strcmp(sat.talker, "GB") == 0 || strcmp(sat.talker, "BD") == 0) {
      ++m_state.beidouCount;
    } else if (strcmp(sat.talker, "GI") == 0) {
      ++m_state.navicCount;
    } else if (strcmp(sat.talker, "GQ") == 0) {
      ++m_state.qzssCount;
    } else if (strcmp(sat.talker, "GS") == 0) {
      ++m_state.sbasCount;
    }
    if (sat.snr > 0) {
      snrSum += sat.snr;
      ++snrCount;
      if (sat.snr > m_state.maxSnr) {
        m_state.maxSnr = sat.snr;
      }
    }
  }
  m_state.meanSnr = snrCount > 0 ? ((float)snrSum / (float)snrCount) : -1.0f;
}

void GnssManager::addUsedPrn(uint8_t prn) {
  if (prn == 0) {
    return;
  }
  for (size_t i = 0; i < m_usedPrnCount; ++i) {
    if (m_usedPrns[i] == prn) {
      return;
    }
  }
  if (m_usedPrnCount < sizeof(m_usedPrns) / sizeof(m_usedPrns[0])) {
    m_usedPrns[m_usedPrnCount++] = prn;
  }
}

void GnssManager::parseGsv(char* fields[], int count) {
  if (count < 4) {
    return;
  }
  const int totalMsgs = atoi(fields[1]);
  const int msgNum = atoi(fields[2]);
  const int satsInView = atoi(fields[3]);
  if (msgNum == 1) {
    char talker[3];
    copyTalker(fields[0], talker);
    gnssDiagf("GSV %s %d/%d sats_in_view=%d", talker, msgNum, totalMsgs, satsInView);
    clearTalkerSatellites(talker);
  } else {
    char talker[3];
    copyTalker(fields[0], talker);
    gnssDiagf("GSV %s %d/%d sats_in_view=%d", talker, msgNum, totalMsgs, satsInView);
  }
  if (satsInView >= 0) {
    m_state.satsInView = satsInView;
  }
  char talker[3];
  copyTalker(fields[0], talker);
  for (int i = 4; i + 3 < count && m_satCount < kMaxSatellites; i += 4) {
    if (!fields[i] || !fields[i][0]) {
      continue;
    }
    SatelliteInfo& sat = m_satellites[m_satCount++];
    sat.valid = true;
    sat.talker[0] = talker[0];
    sat.talker[1] = talker[1];
    sat.talker[2] = '\0';
    sat.prn = (uint8_t)atoi(fields[i]);
    sat.usedInSolution = prnUsedInSolution(sat.prn);
    sat.elevation = (uint8_t)atoi(fields[i + 1]);
    sat.azimuth = (uint16_t)atoi(fields[i + 2]);
    sat.snr = (uint8_t)atoi(fields[i + 3]);
  }
  if (msgNum == totalMsgs) {
    finalizeSatelliteStats();
  }
  m_lastGsvMs = millis();
}

bool GnssManager::prnUsedInSolution(uint8_t prn) const {
  for (size_t i = 0; i < m_usedPrnCount; ++i) {
    if (m_usedPrns[i] == prn) {
      return true;
    }
  }
  return false;
}

void GnssManager::parseRmc(char* fields[], int count) {
  if (count < 10) {
    return;
  }
  const char* utc = fields[1];
  const char* status = fields[2];
  if (status && status[0] == 'A') {
    m_state.validFix = true;
    m_lastFixMs = millis();
  }
  if (utc && strlen(utc) >= 6 && fields[9] && strlen(fields[9]) >= 6) {
    uint8_t hh = 0, mm = 0, ss = 0, dd = 0, mo = 0, yy = 0;
    if (parseUInt2(utc + 0, hh) && parseUInt2(utc + 2, mm) && parseUInt2(utc + 4, ss) &&
        parseUInt2(fields[9] + 0, dd) && parseUInt2(fields[9] + 2, mo) && parseUInt2(fields[9] + 4, yy)) {
      m_state.hour = hh;
      m_state.minute = mm;
      m_state.second = ss;
      m_state.day = dd;
      m_state.month = mo;
      m_state.year = (uint16_t)(2000U + yy);
      m_state.validTime = true;
    }
  }
  if (fields[3] && fields[3][0] && fields[5] && fields[5][0]) {
    m_state.latitude = parseNmeaCoord(fields[3], fields[4]);
    m_state.longitude = parseNmeaCoord(fields[5], fields[6]);
    m_state.validLocation = true;
  }
  if (fields[7] && fields[7][0]) {
    m_state.speedMps = (float)(atof(fields[7]) * 0.514444);
    m_state.validSpeed = true;
  }
  if (fields[8] && fields[8][0]) {
    m_state.courseDeg = atof(fields[8]);
    m_state.validCourse = true;
  }
}

void GnssManager::parseVtg(char* fields[], int count) {
  if (count > 1 && fields[1] && fields[1][0]) {
    m_state.courseDeg = atof(fields[1]);
    m_state.validCourse = true;
  }
  if (count > 7 && fields[7] && fields[7][0]) {
    m_state.speedMps = (float)(atof(fields[7]) / 3.6);
    m_state.validSpeed = true;
  }
}

void GnssManager::parseTxt(char* fields[], int count) {
  if (count <= 4 || !fields[4]) {
    return;
  }
  String text(fields[4]);
  text.toUpperCase();
  if (text.indexOf("L76K") >= 0 || text.indexOf("QUECTEL") >= 0) {
    strlcpy(m_detectedChip, "L76K", sizeof(m_detectedChip));
  }
}

void GnssManager::processNmeaLine(char* line) {
  if (!line || line[0] != '$') {
    return;
  }
  m_sawSentence = true;
  m_state.sawSentence = true;
  if (strstr(line, "GSV") || strstr(line, "GSA")) {
    gnssDiagf("raw %s", line);
  }
  char* star = strchr(line, '*');
  if (star) {
    *star = '\0';
  }
  char* fields[32] = {0};
  int count = splitCsvPreserveEmpty(line, fields, 32);
  if (count <= 0 || !fields[0]) {
    return;
  }
  if (strcmp(fields[0], "$PUBX") == 0) {
    m_seenUbloxPubx = true;
    strlcpy(m_detectedChip, "MAX-M10S", sizeof(m_detectedChip));
    return;
  }
  size_t n = strlen(fields[0]);
  if (n < 6) {
    return;
  }
  const char* type = fields[0] + (n - 3);
  if (strcmp(type, "GGA") == 0) {
    parseGga(fields, count);
  } else if (strcmp(type, "GSA") == 0) {
    parseGsa(fields, count);
  } else if (strcmp(type, "GSV") == 0) {
    parseGsv(fields, count);
  } else if (strcmp(type, "RMC") == 0) {
    parseRmc(fields, count);
  } else if (strcmp(type, "VTG") == 0) {
    parseVtg(fields, count);
  } else if (strcmp(type, "TXT") == 0) {
    parseTxt(fields, count);
  }
}

void GnssManager::poll() {
#ifdef GPS_1PPS_PIN
  m_hasPps = (digitalRead(GPS_1PPS_PIN) == HIGH);
#endif
  while (m_serial.available() > 0) {
    char c = (char)m_serial.read();
    if (c == '\r') {
      continue;
    }
    if (c == '\n') {
      if (m_lineLen > 0) {
        m_line[m_lineLen] = '\0';
        processNmeaLine(m_line);
        m_lineLen = 0;
      }
      continue;
    }
    if (m_lineLen + 1 < sizeof(m_line)) {
      m_line[m_lineLen++] = c;
    } else {
      m_lineLen = 0;
    }
  }
}

GnssSample GnssManager::makeSample() const {
  GnssSample sample = m_state;
  sample.ppsSeen = m_hasPps;
  sample.sampleMillis = millis();
  if (m_lastFixMs > 0) {
    sample.ageOfFixMs = millis() - m_lastFixMs;
  }
  sample.ttffMs = (m_lastFixMs > 0) ? (m_lastFixMs - m_bootMs) : 0;
  if (sample.fixType == FixType::NoFix) {
    if (sample.fixDimension >= 3) {
      sample.fixType = FixType::Fix3D;
    } else if (sample.fixDimension == 2) {
      sample.fixType = FixType::Fix2D;
    }
  }
  return sample;
}

size_t GnssManager::copySatellites(SatelliteInfo* out, size_t maxCount) const {
  if (!out || maxCount == 0) {
    return 0;
  }
  size_t n = m_satCount < maxCount ? m_satCount : maxCount;
  for (size_t i = 0; i < n; ++i) {
    out[i] = m_satellites[i];
  }
  return n;
}

const char* GnssManager::detectedChipName() const {
  return m_detectedChip;
}

}  // namespace field_qa
