#include <WiFi.h>
#include <Wire.h>
#include <time.h>
#include <APRS-IS.h>
#include <APRS-Decoder.h>
#include <U8g2lib.h>
#include <memory>

// ============== DISPLAY ==============
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);

// ============== WIFI ==============
const char* WIFI_SSID1 = "Keenetic-3222";
const char* WIFI_PASS1 = "28021980";
const char* WIFI_SSID2 = "TP-Link_45EC";
const char* WIFI_PASS2 = "16314733";

// ============== APRS-IS ==============
const char* APRS_USER   = "UA1ZBE-5";
const char* APRS_PASS   = "21948";
const char* APRS_SERVER = "rotate.radiorubka.org";
const int   APRS_PORT   = 14580;
const char* APRS_FILTER = "r/67.22/032.29/300";

APRS_IS aprs(APRS_USER, APRS_PASS, "ESP32-APRS", "1.0");

// ============== HOME QTH ==============
const float HOME_LAT = 67.3709;
const float HOME_LON = 32.4896;

// ============== STATION STORAGE ==============
#define MAX_STATIONS 15

struct Station {
  String callsign;
  float lat, lon;
  bool hasPos;
  float distance;
  int heading;
  String comment;
  unsigned long rxTime;
  bool isWx;
  float wxTemp;
  int wxHumidity;
  float wxPressure;
  int speed;
  bool isMobile;
};

Station stations[MAX_STATIONS];
int stationCount = 0;
int displayIdx = 0;

// ============== STATS ==============
unsigned long totalPackets = 0;
unsigned long lastPacketTime = 0;

// ============== FSM ==============
enum State { STATE_SPLASH, STATE_RUNNING };
State state = STATE_SPLASH;
unsigned long splashStart = 0;

// ============== GEO HELPERS ==============
float toRad(float d) { return d * DEG_TO_RAD; }

float haversine(float lat1, float lon1, float lat2, float lon2) {
  double dlat = toRad(lat2 - lat1);
  double dlon = toRad(lon2 - lon1);
  double a = sin(dlat / 2) * sin(dlat / 2) +
             cos(toRad(lat1)) * cos(toRad(lat2)) *
             sin(dlon / 2) * sin(dlon / 2);
  return 6371.0 * 2.0 * atan2(sqrt(a), sqrt(1 - a));
}

int bearing(float lat1, float lon1, float lat2, float lon2) {
  double dlon = toRad(lon2 - lon1);
  double y = sin(dlon) * cos(toRad(lat2));
  double x = cos(toRad(lat1)) * sin(toRad(lat2)) -
             sin(toRad(lat1)) * cos(toRad(lat2)) * cos(dlon);
  return ((int)(atan2(y, x) * RAD_TO_DEG + 0.5) + 360) % 360;
}

const char* dirSymbol(int deg) {
  if (deg >= 337 || deg < 23) return "N";
  if (deg < 68)  return "NE";
  if (deg < 113) return "E";
  if (deg < 158) return "SE";
  if (deg < 203) return "S";
  if (deg < 248) return "SW";
  if (deg < 293) return "W";
  return "NW";
}

// ============== WX PARSER ==============
bool parseWxData(const String& body, float& tempC, int& humidity, float& pressure) {
  bool found = false;
  tempC = -999;
  humidity = -1;
  pressure = -1;

  if (body.length() > 0 && body[0] == '_') found = true;

  int tIdx = body.indexOf('t');
  if (tIdx >= 0 && tIdx + 4 <= body.length()) {
    if (isDigit(body[tIdx + 1]) && isDigit(body[tIdx + 2]) && isDigit(body[tIdx + 3])) {
      int raw = body.substring(tIdx + 1, tIdx + 4).toInt();
      if (raw > -50 && raw < 200) {
        tempC = (raw - 32) * 5.0 / 9.0;
        found = true;
      }
    }
  }

  int hIdx = body.indexOf('h');
  if (hIdx >= 0 && hIdx + 3 <= body.length()) {
    if (isDigit(body[hIdx + 1]) && isDigit(body[hIdx + 2])) {
      humidity = body.substring(hIdx + 1, hIdx + 3).toInt();
    }
  }

  int bIdx = body.indexOf('b');
  if (bIdx >= 0 && bIdx + 6 <= body.length()) {
    bool ok = true;
    for (int i = 1; i <= 5; i++) {
      if (!isDigit(body[bIdx + i])) { ok = false; break; }
    }
    if (ok) {
      pressure = body.substring(bIdx + 1, bIdx + 6).toInt() / 10.0;
    }
  }

  return found;
}

int parseSpeed(const String& body) {
  int len = body.length();
  if (len > 80) len = 80;
  for (int i = 0; i < len - 7; i++) {
    if (isDigit(body[i]) && isDigit(body[i + 1]) && isDigit(body[i + 2]) &&
        body[i + 3] == '/' &&
        isDigit(body[i + 4]) && isDigit(body[i + 5]) && isDigit(body[i + 6])) {
      int c = body.substring(i, i + 3).toInt();
      int s = body.substring(i + 4, i + 7).toInt();
      if (c >= 0 && c <= 360 && s >= 0 && s <= 999) return s;
    }
  }
  return -1;
}

// ============== APRS PARSER ==============
bool parsePosition(const String& data, float& lat, float& lon) {
  int nsIdx = -1;
  for (int i = 0; i < data.length() - 7; i++) {
    if ((data[i] == 'N' || data[i] == 'S') && i >= 7) {
      if (isDigit(data[i - 1]) && isDigit(data[i - 2]) &&
          data[i - 3] == '.' &&
          isDigit(data[i - 4]) && isDigit(data[i - 5]) &&
          isDigit(data[i - 6]) && isDigit(data[i - 7])) {
        nsIdx = i;
        break;
      }
    }
  }
  if (nsIdx < 0) return false;

  lat = data.substring(nsIdx - 7, nsIdx - 5).toFloat() +
        data.substring(nsIdx - 5, nsIdx + 1).toFloat() / 60.0;
  if (data[nsIdx] == 'S') lat = -lat;

  int lonStart = nsIdx + 1;
  if (lonStart + 8 > data.length()) return false;
  if (!isDigit(data[lonStart]) || !isDigit(data[lonStart + 1]) ||
      !isDigit(data[lonStart + 2]) || !isDigit(data[lonStart + 3]) ||
      !isDigit(data[lonStart + 4]) || data[lonStart + 5] != '.' ||
      !isDigit(data[lonStart + 6]) || !isDigit(data[lonStart + 7]))
    return false;

  lon = data.substring(lonStart, lonStart + 3).toFloat() +
        data.substring(lonStart + 3, lonStart + 8).toFloat() / 60.0;

  int ewPos = lonStart + 8;
  if (ewPos < data.length() && data[ewPos] == 'W') lon = -lon;
  return ewPos < data.length() && (data[ewPos] == 'E' || data[ewPos] == 'W');
}

String stripPosition(const String& body) {
  String r = body;
  if (r.length() == 0) return r;
  char c = r[0];
  if (c == '!' || c == '/' || c == '@' || c == '=' || c == '>' ||
      c == '_' || c == '\'' || c == '`' || c == ':' || c == ';') {
    r = r.substring(1);
  }

  int nsIdx = -1, ewEnd = -1;
  for (int i = 0; i < r.length() - 7; i++) {
    if ((r[i] == 'N' || r[i] == 'S') && i >= 7) {
      if (isDigit(r[i - 1]) && isDigit(r[i - 2]) && r[i - 3] == '.' &&
          isDigit(r[i - 4]) && isDigit(r[i - 5]) &&
          isDigit(r[i - 6]) && isDigit(r[i - 7])) {
        nsIdx = i;
        break;
      }
    }
  }
  if (nsIdx >= 0) {
    for (int j = nsIdx + 9; j < r.length(); j++) {
      if (r[j] == 'E' || r[j] == 'W') { ewEnd = j + 1; break; }
    }
    if (ewEnd > 0) {
      r = r.substring(ewEnd);
      while (r.length() > 0 && r[0] < 0x20) r = r.substring(1);
      if (r.length() > 0 && r[0] > 0x1f && r[0] < 0x40) r = r.substring(1);
      if (r.length() > 0 && r[0] > 0x1f && r[0] < 0x60) r = r.substring(1);
    }
  }
  r.trim();
  return r;
}

// ============== STATION MANAGEMENT ==============
int findStation(const String& call) {
  for (int i = 0; i < stationCount; i++)
    if (stations[i].callsign == call) return i;
  return -1;
}

int addStation(const String& call) {
  int idx = findStation(call);
  if (idx >= 0) return idx;
  if (stationCount < MAX_STATIONS) return stationCount++;
  idx = displayIdx;
  return idx;
}

void handleMessage(std::shared_ptr<APRSMessage> msg) {
  totalPackets++;
  lastPacketTime = millis();

  String src = msg->getSource();
  String body = msg->getAPRSBody()->getData();

  int idx = addStation(src);
  Station& s = stations[idx];
  s.callsign = src;
  s.rxTime = millis();
  s.hasPos = parsePosition(body, s.lat, s.lon);
  s.comment = stripPosition(body);
  s.isWx = parseWxData(body, s.wxTemp, s.wxHumidity, s.wxPressure);
  s.speed = parseSpeed(body);
  s.isMobile = s.speed > 0;

  if (s.hasPos) {
    s.distance = haversine(HOME_LAT, HOME_LON, s.lat, s.lon);
    s.heading = bearing(HOME_LAT, HOME_LON, s.lat, s.lon);
  }

  displayIdx = idx;

  Serial.print("RX: ");
  Serial.print(src);
  if (s.hasPos) {
    Serial.print(" ");
    Serial.print(s.distance, 1);
    Serial.print("km ");
    Serial.print(s.heading);
    Serial.print("\xB0");
  }
  if (s.comment.length() > 0) {
    Serial.print(" \"");
    Serial.print(s.comment.substring(0, 40));
    Serial.print("\"");
  }
  Serial.println();
}

// ============== DISPLAY ==============
void drawSplash() {
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_10x20_t_cyrillic);
  int w = u8g2.getUTF8Width("APRS");
  u8g2.drawUTF8((128 - w) / 2, 28, "APRS");

  u8g2.setFont(u8g2_font_6x12_t_cyrillic);
  u8g2.drawUTF8(0, 44, APRS_USER);

  String ver = "v1.0";
  int vw = u8g2.getUTF8Width(ver.c_str());
  u8g2.drawUTF8(128 - vw, 44, ver.c_str());
  u8g2.sendBuffer();
}

String getTimeStr() {
  struct tm t;
  if (!getLocalTime(&t)) return "--:--";
  char buf[6];
  snprintf(buf, sizeof(buf), "%02d:%02d", t.tm_hour, t.tm_min);
  return String(buf);
}

void drawStatusBar() {
  u8g2.setFont(u8g2_font_6x12_t_cyrillic);
  u8g2.drawUTF8(2, 10, getTimeStr().c_str());

  String right = String(totalPackets) + "pkt";
  int rw = u8g2.getUTF8Width(right.c_str());
  u8g2.drawUTF8(126 - rw, 10, right.c_str());
}

void drawWxStation(int idx) {
  drawStatusBar();
  u8g2.drawLine(0, 14, 127, 14);

  Station& s = stations[idx];

  u8g2.setDrawColor(1);
  u8g2.drawRBox(2, 16, 18, 14, 2);
  u8g2.setDrawColor(0);
  u8g2.setFont(u8g2_font_6x12_t_cyrillic);
  u8g2.drawUTF8(5, 28, "WX");
  u8g2.setDrawColor(1);

  u8g2.setFont(u8g2_font_10x20_t_cyrillic);
  u8g2.drawUTF8(24, 34, s.callsign.c_str());

  if (s.hasPos) {
    u8g2.setFont(u8g2_font_6x12_t_cyrillic);
    char line[24];
    snprintf(line, sizeof(line), "%s %d\xB0 %.0fkm",
             dirSymbol(s.heading), s.heading, s.distance);
    int w = u8g2.getUTF8Width(line);
    u8g2.drawUTF8(126 - w, 34, line);
  }

  u8g2.setFont(u8g2_font_6x12_t_cyrillic);
  int y = 50;

  if (s.wxTemp > -100 && s.wxTemp < 150) {
    String t = "Temp: " + String(s.wxTemp, 1) + "\xB0" "C";
    int tw = u8g2.getUTF8Width(t.c_str());
    int tx = (128 - tw) / 2;
    if (tx < 2) tx = 2;
    u8g2.drawUTF8(tx, y, t.c_str());
  }

  y += 14;

  String wl;
  float pressMmHg = s.wxPressure * 0.75;
  if (s.wxHumidity >= 0 && s.wxPressure > 0) {
    wl = "Hum: " + String(s.wxHumidity) + "%  " + String(pressMmHg, 0) + "mm";
  } else if (s.wxHumidity >= 0) {
    wl = "Humidity: " + String(s.wxHumidity) + "%";
  } else if (s.wxPressure > 0) {
    wl = "Press: " + String(pressMmHg, 0) + "mm";
  } else {
    unsigned long sec = (millis() - s.rxTime) / 1000;
    wl = sec < 120 ? String(sec) + "s" : String(sec / 60) + "m";
    wl = "RX: " + wl + " ago";
  }
  int ww = u8g2.getUTF8Width(wl.c_str());
  int wx = (128 - ww) / 2;
  if (wx < 2) wx = 2;
  u8g2.drawUTF8(wx, y, wl.c_str());
}

const unsigned char person_bits[] = {
  0x0C, 0x00,
  0x0C, 0x00,
  0x1E, 0x00,
  0x0C, 0x00,
  0x1E, 0x00,
  0x33, 0x00,
  0x21, 0x00,
  0x40, 0x80,
  0x40, 0x80,
  0x01, 0x40,
  0x00, 0x80,
  0x00, 0x00,
};

void drawStationScreen(int idx) {
  Station& s = stations[idx];
  if (s.isWx) {
    drawWxStation(idx);
    return;
  }

  drawStatusBar();
  u8g2.drawLine(0, 14, 127, 14);

  if (stationCount == 0) {
    u8g2.setFont(u8g2_font_6x12_t_cyrillic);
    u8g2.drawUTF8(2, 50, "Waiting for packets...");
    return;
  }

  int cx = 2;
  if (s.isMobile) {
    u8g2.drawXBM(cx, 16, 12, 12, person_bits);
    cx = 18;
  }

  u8g2.setFont(u8g2_font_10x20_t_cyrillic);
  u8g2.drawUTF8(cx, 34, s.callsign.c_str());

  u8g2.setFont(u8g2_font_6x12_t_cyrillic);
  int y = 50;

  if (s.isMobile) {
    float kmh = s.speed * 1.852;
    String sp = "Speed: " + String(kmh, 0) + " km/h";
    u8g2.drawUTF8(2, y, sp.c_str());

    char dirAge[16];
    unsigned long sec = (millis() - s.rxTime) / 1000;
    String age = sec < 120 ? String(sec) + "s" : String(sec / 60) + "m";
    snprintf(dirAge, sizeof(dirAge), "%s%d\xB0 %s", dirSymbol(s.heading), s.heading, age.c_str());
    u8g2.drawUTF8(126 - u8g2.getUTF8Width(dirAge), y, dirAge);

    y += 14;
    String di = "Dist: " + String(s.distance, 1) + " km";
    u8g2.drawUTF8(2, y, di.c_str());
  } else if (s.hasPos) {
    char line[32];
    snprintf(line, sizeof(line), "%s %d\xB0  %.1f km",
             dirSymbol(s.heading), s.heading, s.distance);
    u8g2.drawUTF8(2, y, line);

    unsigned long sec = (millis() - s.rxTime) / 1000;
    String age;
    if (sec < 120) age = String(sec) + "s";
    else age = String(sec / 60) + "m";
    int aw = u8g2.getUTF8Width(age.c_str());
    u8g2.drawUTF8(126 - aw, y, age.c_str());

    y += 14;
  } else if (s.comment.length() > 0) {
    String c = s.comment;
    if (u8g2.getUTF8Width(c.c_str()) > 124) {
      while (u8g2.getUTF8Width(c.c_str()) > 120 && c.length() > 0)
        c = c.substring(0, c.length() - 1);
      c += "...";
    }
    u8g2.drawUTF8(2, y, c.c_str());
  }
}

void showConnecting() {
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_10x20_t_cyrillic);
  u8g2.drawUTF8(2, 28, "Connecting...");
  u8g2.setFont(u8g2_font_6x12_t_cyrillic);
  if (WiFi.status() != WL_CONNECTED) {
    u8g2.drawUTF8(2, 48, "WiFi");
  } else {
    u8g2.drawUTF8(2, 48, "APRS-IS");
    u8g2.drawUTF8(80, 48, APRS_SERVER);
  }
  u8g2.sendBuffer();
}

// ============== CONNECTION ==============
bool connectWiFi() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID1, WIFI_PASS1);
  for (int i = 0; i < 30 && WiFi.status() != WL_CONNECTED; i++) {
    showConnecting();
    delay(500);
  }
  if (WiFi.status() != WL_CONNECTED) {
    WiFi.disconnect(true);
    delay(100);
    WiFi.begin(WIFI_SSID2, WIFI_PASS2);
    for (int i = 0; i < 30 && WiFi.status() != WL_CONNECTED; i++) {
      showConnecting();
      delay(500);
    }
  }
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("WiFi OK: " + WiFi.localIP().toString());
    return true;
  }
  Serial.println("WiFi FAIL");
  return false;
}

void connectAPRS() {
  showConnecting();
  if (aprs.connect(APRS_SERVER, APRS_PORT, APRS_FILTER)) {
    Serial.println("APRS-IS connected to " + String(APRS_SERVER));
  } else {
    Serial.println("APRS-IS connection failed");
  }
}

// ============== SETUP ==============
void setup() {
  Serial.begin(115200);
  Wire.begin();

  u8g2.begin();
  u8g2.enableUTF8Print();

  drawSplash();
  splashStart = millis();
  state = STATE_SPLASH;

  WiFi.persistent(false);
  lastPacketTime = millis();

  if (connectWiFi()) {
    configTime(10800, 0, "pool.ntp.org", "time.nist.gov");
    connectAPRS();
  } else {
    // will retry in loop()
  }
}

// ============== LOOP ==============
void loop() {
  if (state == STATE_SPLASH && millis() - splashStart > 3000) {
    state = STATE_RUNNING;
  }

  if (aprs.connected() && aprs.available() > 0) {
    auto msg = aprs.getAPRSMessage();
    if (msg != nullptr) handleMessage(msg);
  }

  {
    static unsigned long lastWifiTry = 0;
    if (WiFi.status() != WL_CONNECTED && millis() - lastWifiTry > 30000) {
      lastWifiTry = millis();
      connectWiFi();
      configTime(10800, 0, "pool.ntp.org", "time.nist.gov");
    }
  }
  {
    static unsigned long lastAprsTry = 0;
    if (WiFi.status() == WL_CONNECTED && !aprs.connected() && millis() - lastAprsTry > 30000) {
      lastAprsTry = millis();
      connectAPRS();
    }
  }

  u8g2.clearBuffer();
  if (state == STATE_SPLASH) {
    u8g2.setFont(u8g2_font_6x12_t_cyrillic);
    u8g2.drawUTF8(2, 60, "Connecting...");
    u8g2.sendBuffer();
  } else {
    drawStationScreen(displayIdx);
    u8g2.sendBuffer();
  }

  delay(30);
}