#include "TBeamSupremeLoRaInterface.h"
#include "TBeamDisplay.h"
#include "tbeam_supreme_adapter.h"

#include <Arduino.h>
#include <Destination.h>
#include <Identity.h>
#include <Link.h>
#include <Log.h>
#include <Packet.h>
#include <Reticulum.h>
#include <Transport.h>
#include <Type.h>
#include <Utilities/OS.h>
#include <SD.h>
#include <SPI.h>
#include <Wire.h>
#include <XPowersLib.h>
#include <microStore/Adapters/UniversalFileSystem.h>
#include <microStore/FileSystem.h>

#ifndef NODE_LABEL
#define NODE_LABEL "?"
#endif

#ifndef BOARD_ID
#define BOARD_ID "UNKNOWN"
#endif

#ifndef LOCAL_IDENTITY_HEX
#error "LOCAL_IDENTITY_HEX must be supplied by scripts/set_build_identity.py"
#endif

#ifndef FW_BUILD_UTC
#define FW_BUILD_UTC "unknown"
#endif

#ifndef SIM_PHY_ENVELOPE
#define SIM_PHY_ENVELOPE 0
#endif

#ifndef SIM_PHY_BLOCK_BOB_CY
#define SIM_PHY_BLOCK_BOB_CY 0
#endif

#ifndef MR_LINKFWD_DELAY_MS
#define MR_LINKFWD_DELAY_MS 0
#endif
#ifndef ANNOUNCEMENT_2
#define ANNOUNCEMENT_2 300
#endif
#ifndef ANNOUNCEMENT_REPEAT
#define ANNOUNCEMENT_REPEAT 3600
#endif

static constexpr const char* APP_NAME = "microreticulum";
static constexpr const char* APP_ASPECT = "linkping";
static constexpr const char* ANNOUNCE_FILTER = "microreticulum.linkping";
static constexpr const char* CLOCK_MARKER_PATH = "/ex205/clock.txt";
static constexpr uint32_t DISCIPLINE_HOLDOVER_SECONDS = 24UL * 60UL * 60UL;
static constexpr uint32_t GPS_STATUS_PERIOD_MS = 2000;
static constexpr uint32_t PPS_WAIT_MS = 1500;
static constexpr uint32_t LINK_RETRY_MS = 60000;
static constexpr uint32_t LINK_RETRY_WINDOW_MS = 180000;
static constexpr uint32_t LINK_RX_STALE_MS = 75000;
static constexpr uint32_t LINK_REOPEN_DELAY_MS = 5000;
static constexpr uint8_t LINK_MAX_ATTEMPTS_PER_WINDOW = 3;
static constexpr uint32_t ANNOUNCEMENT_2_MS = (uint32_t)ANNOUNCEMENT_2 * 1000UL;
static constexpr uint32_t ANNOUNCEMENT_REPEAT_MS = (uint32_t)ANNOUNCEMENT_REPEAT * 1000UL;

struct DateTime {
  uint16_t year = 0;
  uint8_t month = 0;
  uint8_t day = 0;
  uint8_t hour = 0;
  uint8_t minute = 0;
  uint8_t second = 0;
};

struct GpsState {
  bool valid_time = false;
  bool valid_fix = false;
  int sats_used = -1;
  DateTime utc;
  uint32_t last_time_ms = 0;
};

static RNS::Reticulum reticulum({RNS::Type::NONE});
static RNS::Interface lora_interface({RNS::Type::NONE});
static RNS::Identity local_identity({RNS::Type::NONE});
static RNS::Identity transport_identity({RNS::Type::NONE});
static RNS::Destination inbound_destination({RNS::Type::NONE});
static bool clock_ready = false;
static bool sd_ready = false;
static TBeamSupremeLoRaInterface* lora_impl = nullptr;
static XPowersLibInterface* pmu = nullptr;
static tbeam::TBeamDisplay oled_display;
static SPIClass sd_spi(FSPI);
static HardwareSerial gps_serial(1);
static GpsState gps;
static char gps_line[128];
static size_t gps_line_len = 0;
static volatile uint32_t last_pps_ms = 0;

static constexpr uint8_t MAX_PEERS = 6;

struct PeerState {
  String label;
  RNS::Bytes destination_hash;
  RNS::Destination destination = {RNS::Type::NONE};
  RNS::Link outbound_link = {RNS::Type::NONE};
  RNS::Link inbound_link = {RNS::Type::NONE};
  bool announced = false;
  bool outbound_attempted = false;
  bool outbound_active = false;
  bool inbound_active = false;
  bool outbound_failed = false;
  uint8_t outbound_attempts = 0;
  uint32_t outbound_attempt_window_ms = 0;
  uint32_t last_link_attempt_ms = 0;
  uint32_t last_link_active_ms = 0;
  uint32_t next_link_open_ms = 0;
  uint32_t last_rx_ms = 0;
  uint32_t last_tx_ms = 0;
  uint32_t tx_iter = 0;
  uint8_t last_tx_second = 255;
};

static PeerState peers[MAX_PEERS];

static void on_link_packet(const RNS::Bytes& data, const RNS::Packet& packet);
static void on_link_closed(RNS::Link& link);

static const char* node_label_for_slot(uint8_t slot) {
  switch (slot) {
    case 0: return "Amy";
    case 1: return "Bob";
    case 2: return "Cy";
    case 3: return "Dan";
    case 4: return "Ed";
    case 5: return "Flo";
    case 6: return "Guy";
    default: return "unknown";
  }
}

static const char* board_id_for_label(const String& label) {
  if (label == "Amy") {
    return "AMY";
  }
  if (label == "Bob") {
    return "BOB";
  }
  if (label == "Cy") {
    return "CY";
  }
  if (label == "Dan") {
    return "DAN";
  }
  if (label == "Ed") {
    return "ED";
  }
  if (label == "Flo") {
    return "FLO";
  }
  if (label == "Guy") {
    return "GUY";
  }
  return label.c_str();
}

static bool is_local_name(const String& value) {
  return value == NODE_LABEL || value == BOARD_ID;
}

static String peer_label_from_name(const String& value) {
  if (value == "AMY") {
    return "Amy";
  }
  if (value == "BOB") {
    return "Bob";
  }
  if (value == "CY") {
    return "Cy";
  }
  if (value == "DAN") {
    return "Dan";
  }
  if (value == "ED") {
    return "Ed";
  }
  if (value == "FLO") {
    return "Flo";
  }
  if (value == "GUY") {
    return "Guy";
  }
  return value;
}

static void IRAM_ATTR on_pps_edge() {
  last_pps_ms = millis();
}

static uint8_t to_bcd(uint8_t value) {
  return (uint8_t)(((value / 10U) << 4U) | (value % 10U));
}

static uint8_t from_bcd(uint8_t value) {
  return (uint8_t)(((value >> 4U) * 10U) + (value & 0x0FU));
}

static bool is_leap_year(uint16_t year) {
  return ((year % 4U) == 0U && (year % 100U) != 0U) || ((year % 400U) == 0U);
}

static uint8_t days_in_month(uint16_t year, uint8_t month) {
  static const uint8_t days[12] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  if (month == 2U) {
    return is_leap_year(year) ? 29U : 28U;
  }
  if (month >= 1U && month <= 12U) {
    return days[month - 1U];
  }
  return 0;
}

static bool valid_datetime(const DateTime& dt) {
  return dt.year >= 2024U && dt.year <= 2099U &&
         dt.month >= 1U && dt.month <= 12U &&
         dt.day >= 1U && dt.day <= days_in_month(dt.year, dt.month) &&
         dt.hour <= 23U && dt.minute <= 59U && dt.second <= 59U;
}

static int64_t days_from_civil(int year, unsigned month, unsigned day) {
  year -= (month <= 2U);
  const int era = (year >= 0 ? year : year - 399) / 400;
  const unsigned yoe = (unsigned)(year - era * 400);
  const unsigned doy = (153U * (month + (month > 2U ? (unsigned)-3 : 9U)) + 2U) / 5U + day - 1U;
  const unsigned doe = yoe * 365U + yoe / 4U - yoe / 100U + doy;
  return era * 146097 + (int)doe - 719468;
}

static int64_t to_epoch_seconds(const DateTime& dt) {
  const int64_t days = days_from_civil((int)dt.year, dt.month, dt.day);
  return days * 86400LL + (int64_t)dt.hour * 3600LL + (int64_t)dt.minute * 60LL + dt.second;
}

static bool from_epoch_seconds(int64_t seconds, DateTime& out) {
  if (seconds < 0) {
    return false;
  }

  int64_t days = seconds / 86400LL;
  int64_t rem = seconds % 86400LL;
  out.hour = (uint8_t)(rem / 3600LL);
  rem %= 3600LL;
  out.minute = (uint8_t)(rem / 60LL);
  out.second = (uint8_t)(rem % 60LL);

  days += 719468;
  const int era = (days >= 0 ? days : days - 146096) / 146097;
  const unsigned doe = (unsigned)(days - era * 146097);
  const unsigned yoe = (doe - doe / 1460U + doe / 36524U - doe / 146096U) / 365U;
  int year = (int)yoe + era * 400;
  const unsigned doy = doe - (365U * yoe + yoe / 4U - yoe / 100U);
  const unsigned mp = (5U * doy + 2U) / 153U;
  const unsigned day = doy - (153U * mp + 2U) / 5U + 1U;
  const unsigned month = mp + (mp < 10U ? 3U : (unsigned)-9);
  year += (month <= 2U);

  out.year = (uint16_t)year;
  out.month = (uint8_t)month;
  out.day = (uint8_t)day;
  return valid_datetime(out);
}

static void format_iso(const DateTime& dt, char* out, size_t out_size) {
  snprintf(out, out_size, "%04u-%02u-%02uT%02u:%02u:%02uZ",
           (unsigned)dt.year, (unsigned)dt.month, (unsigned)dt.day,
           (unsigned)dt.hour, (unsigned)dt.minute, (unsigned)dt.second);
}

static bool read_rtc(DateTime& out, int64_t* epoch_out = nullptr) {
  Wire1.beginTransmission(RTC_I2C_ADDR);
  Wire1.write(0x02);
  if (Wire1.endTransmission(false) != 0) {
    return false;
  }

  if (Wire1.requestFrom((int)RTC_I2C_ADDR, 7) != 7) {
    return false;
  }

  const uint8_t sec = Wire1.read();
  const uint8_t min = Wire1.read();
  const uint8_t hour = Wire1.read();
  const uint8_t day = Wire1.read();
  (void)Wire1.read();
  const uint8_t month = Wire1.read();
  const uint8_t year = Wire1.read();

  if (sec & 0x80U) {
    return false;
  }
  out.second = from_bcd(sec & 0x7FU);
  out.minute = from_bcd(min & 0x7FU);
  out.hour = from_bcd(hour & 0x3FU);
  out.day = from_bcd(day & 0x3FU);
  out.month = from_bcd(month & 0x1FU);
  out.year = (uint16_t)((month & 0x80U) ? 1900U : 2000U) + from_bcd(year);
  if (!valid_datetime(out)) {
    return false;
  }
  if (epoch_out) {
    *epoch_out = to_epoch_seconds(out);
  }
  return true;
}

static bool write_rtc(const DateTime& dt) {
  if (!valid_datetime(dt)) {
    return false;
  }
  Wire1.beginTransmission(RTC_I2C_ADDR);
  Wire1.write(0x02);
  Wire1.write(to_bcd(dt.second));
  Wire1.write(to_bcd(dt.minute));
  Wire1.write(to_bcd(dt.hour));
  Wire1.write(to_bcd(dt.day));
  Wire1.write(0x00);
  Wire1.write(to_bcd(dt.month));
  Wire1.write(to_bcd((uint8_t)(dt.year % 100U)));
  return Wire1.endTransmission() == 0;
}

static bool mount_sd() {
  pinMode(tbeam_supreme::sdCs(), OUTPUT);
  digitalWrite(tbeam_supreme::sdCs(), HIGH);
  pinMode(tbeam_supreme::imuCs(), OUTPUT);
  digitalWrite(tbeam_supreme::imuCs(), HIGH);
  sd_spi.begin(tbeam_supreme::sdSck(), tbeam_supreme::sdMiso(), tbeam_supreme::sdMosi(), tbeam_supreme::sdCs());
  sd_ready = SD.begin(tbeam_supreme::sdCs(), sd_spi, 4000000);
  Serial.printf("SD %s\r\n", sd_ready ? "ready" : "not mounted");
  return sd_ready;
}

static int64_t read_clock_marker() {
  if (!sd_ready || !SD.exists(CLOCK_MARKER_PATH)) {
    return 0;
  }
  File f = SD.open(CLOCK_MARKER_PATH, FILE_READ);
  if (!f) {
    return 0;
  }
  int64_t marker_epoch = 0;
  while (f.available()) {
    String line = f.readStringUntil('\n');
    line.trim();
    const int64_t value = (int64_t)strtoll(line.c_str(), nullptr, 10);
    if (value > 0) {
      marker_epoch = value;
    }
  }
  f.close();
  return marker_epoch;
}

static bool write_clock_marker(int64_t epoch) {
  if (!sd_ready) {
    return false;
  }
  SD.mkdir("/ex205");
  if (SD.exists(CLOCK_MARKER_PATH)) {
    SD.remove(CLOCK_MARKER_PATH);
  }
  File f = SD.open(CLOCK_MARKER_PATH, FILE_WRITE);
  if (!f) {
    return false;
  }
  f.printf("%lld\n", (long long)epoch);
  f.close();
  return true;
}

static void show_status(const char* left, const char* right = nullptr, const char* footer = nullptr) {
  oled_display.showStatus("Ex 205 " BOARD_ID, left, right, footer);
}

static void show_splash() {
  oled_display.showLines("Exercise 205", "Build", FW_BUILD_UTC, BOARD_ID, NODE_LABEL, "sustained");
}

static bool hex_nibble(char c, uint8_t& out) {
  if (c >= '0' && c <= '9') {
    out = (uint8_t)(c - '0');
    return true;
  }
  if (c >= 'a' && c <= 'f') {
    out = (uint8_t)(10 + c - 'a');
    return true;
  }
  if (c >= 'A' && c <= 'F') {
    out = (uint8_t)(10 + c - 'A');
    return true;
  }
  return false;
}

static bool identity_bytes_from_hex(RNS::Bytes& out) {
  const char* hex = LOCAL_IDENTITY_HEX;
  const size_t len = strlen(hex);
  if (len == 0 || (len % 2U) != 0U) {
    return false;
  }
  uint8_t buffer[96];
  const size_t byte_count = len / 2U;
  if (byte_count > sizeof(buffer)) {
    return false;
  }
  for (size_t i = 0; i < byte_count; ++i) {
    uint8_t hi = 0;
    uint8_t lo = 0;
    if (!hex_nibble(hex[i * 2U], hi) || !hex_nibble(hex[i * 2U + 1U], lo)) {
      return false;
    }
    buffer[i] = (uint8_t)((hi << 4U) | lo);
  }
  out = RNS::Bytes(buffer, byte_count);
  return true;
}

static bool parse_utc_time(const char* value, DateTime& out) {
  if (!value || strlen(value) < 6) {
    return false;
  }
  out.hour = (uint8_t)((value[0] - '0') * 10 + (value[1] - '0'));
  out.minute = (uint8_t)((value[2] - '0') * 10 + (value[3] - '0'));
  out.second = (uint8_t)((value[4] - '0') * 10 + (value[5] - '0'));
  return out.hour <= 23U && out.minute <= 59U && out.second <= 59U;
}

static bool parse_utc_date(const char* value, DateTime& out) {
  if (!value || strlen(value) < 6) {
    return false;
  }
  out.day = (uint8_t)((value[0] - '0') * 10 + (value[1] - '0'));
  out.month = (uint8_t)((value[2] - '0') * 10 + (value[3] - '0'));
  const uint8_t yy = (uint8_t)((value[4] - '0') * 10 + (value[5] - '0'));
  out.year = (uint16_t)(2000U + yy);
  return true;
}

static void parse_gga(char* line) {
  char* fields[16] = {};
  size_t count = 0;
  char* save = nullptr;
  for (char* tok = strtok_r(line, ",", &save); tok && count < 16; tok = strtok_r(nullptr, ",", &save)) {
    fields[count++] = tok;
  }
  if (count > 7 && fields[7] && fields[7][0]) {
    gps.sats_used = atoi(fields[7]);
  }
}

static void parse_rmc(char* line) {
  char* fields[16] = {};
  size_t count = 0;
  char* save = nullptr;
  for (char* tok = strtok_r(line, ",", &save); tok && count < 16; tok = strtok_r(nullptr, ",", &save)) {
    fields[count++] = tok;
  }
  if (count <= 9) {
    return;
  }
  DateTime dt = gps.utc;
  const bool has_time = parse_utc_time(fields[1], dt);
  const bool active = fields[2] && fields[2][0] == 'A';
  const bool has_date = parse_utc_date(fields[9], dt);
  if (has_time && has_date && valid_datetime(dt)) {
    gps.utc = dt;
    gps.valid_time = true;
    gps.valid_fix = active;
    gps.last_time_ms = millis();
  }
}

static void process_nmea(char* line) {
  if (strncmp(line, "$GPRMC", 6) == 0 || strncmp(line, "$GNRMC", 6) == 0) {
    parse_rmc(line);
  } else if (strncmp(line, "$GPGGA", 6) == 0 || strncmp(line, "$GNGGA", 6) == 0) {
    parse_gga(line);
  }
}

static void poll_gps() {
  while (gps_serial.available() > 0) {
    const char c = (char)gps_serial.read();
    if (c == '\r') {
      continue;
    }
    if (c == '\n') {
      if (gps_line_len > 0) {
        gps_line[gps_line_len] = '\0';
        process_nmea(gps_line);
        gps_line_len = 0;
      }
    } else if (gps_line_len + 1U < sizeof(gps_line)) {
      gps_line[gps_line_len++] = c;
    } else {
      gps_line_len = 0;
    }
  }
}

static bool wait_for_pps(uint32_t timeout_ms) {
  const uint32_t start = millis();
  const uint32_t prior = last_pps_ms;
  while ((uint32_t)(millis() - start) < timeout_ms) {
    if (last_pps_ms != prior) {
      return true;
    }
    poll_gps();
    delay(5);
  }
  return false;
}

static bool saved_clock_is_fresh() {
  DateTime rtc_now{};
  int64_t rtc_epoch = 0;
  if (!read_rtc(rtc_now, &rtc_epoch)) {
    Serial.println("RTC invalid; GPS discipline required");
    return false;
  }
  char iso[32];
  format_iso(rtc_now, iso, sizeof(iso));
  if (!sd_ready) {
    Serial.printf("Clock holdover accepted without SD marker: rtc=%s reason=sd_unavailable\r\n", iso);
    show_status("Clock OK", "RTC only", iso);
    return true;
  }
  const int64_t marker_epoch = read_clock_marker();
  if (marker_epoch <= 0) {
    Serial.println("No saved clock marker; GPS discipline required");
    return false;
  }
  const int64_t age = rtc_epoch - marker_epoch;
  if (age < 0 || age > (int64_t)DISCIPLINE_HOLDOVER_SECONDS) {
    Serial.printf("Saved clock stale: age_s=%lld\r\n", (long long)age);
    return false;
  }
  Serial.printf("Clock holdover accepted: rtc=%s age_s=%lld\r\n", iso, (long long)age);
  (void)write_clock_marker(rtc_epoch);
  show_status("Clock OK", "holdover", iso);
  return true;
}

static void discipline_clock_from_gps() {
  Serial.println("Waiting for GPS UTC before LoRa startup");
  gps_serial.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
#ifdef GPS_1PPS_PIN
  pinMode(GPS_1PPS_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(GPS_1PPS_PIN), on_pps_edge, RISING);
#endif

  uint32_t last_status = 0;
  while (true) {
    poll_gps();
    const uint32_t now = millis();
    if ((uint32_t)(now - last_status) >= GPS_STATUS_PERIOD_MS) {
      last_status = now;
      char sats[16];
      snprintf(sats, sizeof(sats), "sats=%d", gps.sats_used);
      show_status("Take outside", gps.valid_time ? "GPS time" : "No GPS", sats);
      Serial.printf("gps_gate time=%u fix=%u sats=%d pps_ms=%lu\r\n",
                    gps.valid_time ? 1U : 0U,
                    gps.valid_fix ? 1U : 0U,
                    gps.sats_used,
                    (unsigned long)last_pps_ms);
    }

    if (gps.valid_time && gps.valid_fix && (uint32_t)(now - gps.last_time_ms) < 5000U) {
      DateTime disciplined = gps.utc;
      bool used_pps = wait_for_pps(PPS_WAIT_MS);
      if (used_pps) {
        int64_t snapped = to_epoch_seconds(gps.utc) + 1LL;
        (void)from_epoch_seconds(snapped, disciplined);
      }
      if (write_rtc(disciplined)) {
        const int64_t epoch = to_epoch_seconds(disciplined);
        (void)write_clock_marker(epoch);
        char iso[32];
        format_iso(disciplined, iso, sizeof(iso));
        Serial.printf("Clock disciplined from GPS: utc=%s pps=%u marker_written=%u\r\n",
                      iso,
                      used_pps ? 1U : 0U,
                      sd_ready ? 1U : 0U);
        show_status("Clock set", used_pps ? "GPS+PPS" : "GPS", iso);
        delay(1000);
        return;
      }
      Serial.println("RTC write failed; retrying GPS discipline");
    }
    delay(10);
  }
}

static int8_t node_index_for_label(const String& label) {
  if (label == "Amy") {
    return 0;
  }
  if (label == "Bob") {
    return 1;
  }
  if (label == "Cy") {
    return 2;
  }
  if (label == "Dan") {
    return 3;
  }
  if (label == "Ed") {
    return 4;
  }
  if (label == "Flo") {
    return 5;
  }
  if (label == "Guy") {
    return 6;
  }
  return -1;
}

static uint8_t directed_link_open_second(const String& label) {
  int8_t a = node_index_for_label(String(NODE_LABEL));
  int8_t b = node_index_for_label(label);
  if (a < 0 || b < 0) {
    return 1;
  }
  return (uint8_t)(1U + (uint8_t)(((uint8_t)a * 7U + (uint8_t)b) % 29U) * 2U);
}

static uint8_t node_send_slot_second() {
  if (strcmp(NODE_LABEL, "Amy") == 0) {
    return 4;
  }
  if (strcmp(NODE_LABEL, "Bob") == 0) {
    return 8;
  }
  if (strcmp(NODE_LABEL, "Cy") == 0) {
    return 12;
  }
  if (strcmp(NODE_LABEL, "Dan") == 0) {
    return 16;
  }
  if (strcmp(NODE_LABEL, "Ed") == 0) {
    return 20;
  }
  if (strcmp(NODE_LABEL, "Flo") == 0) {
    return 24;
  }
  if (strcmp(NODE_LABEL, "Guy") == 0) {
    return 28;
  }
  return (uint8_t)(4U + ((uint8_t)NODE_SLOT_INDEX * 4U));
}

static int find_peer_by_label(const String& label) {
  for (uint8_t i = 0; i < MAX_PEERS; ++i) {
    if (peers[i].label == label) {
      return i;
    }
  }
  return -1;
}

static int find_peer_by_link_hash(const RNS::Bytes& link_hash) {
  for (uint8_t i = 0; i < MAX_PEERS; ++i) {
    if (peers[i].outbound_link && peers[i].outbound_link.hash() == link_hash) {
      return i;
    }
    if (peers[i].inbound_link && peers[i].inbound_link.hash() == link_hash) {
      return i;
    }
  }
  return -1;
}

static int first_free_peer_slot() {
  for (uint8_t i = 0; i < MAX_PEERS; ++i) {
    if (peers[i].label.length() == 0 && !peers[i].outbound_link && !peers[i].inbound_link) {
      return i;
    }
  }
  return -1;
}

static void clear_peer_slot(uint8_t index) {
  peers[index].label = "";
  peers[index].destination_hash.clear();
  peers[index].destination = {RNS::Type::NONE};
  peers[index].outbound_link = {RNS::Type::NONE};
  peers[index].inbound_link = {RNS::Type::NONE};
  peers[index].announced = false;
  peers[index].outbound_attempted = false;
  peers[index].outbound_active = false;
  peers[index].inbound_active = false;
  peers[index].outbound_failed = false;
  peers[index].outbound_attempts = 0;
  peers[index].outbound_attempt_window_ms = 0;
  peers[index].last_link_attempt_ms = 0;
  peers[index].last_link_active_ms = 0;
  peers[index].next_link_open_ms = 0;
  peers[index].last_rx_ms = 0;
  peers[index].last_tx_ms = 0;
  peers[index].tx_iter = 0;
  peers[index].last_tx_second = 255;
}

static int ensure_peer_for_label(const String& label) {
  int index = find_peer_by_label(label);
  if (index >= 0) {
    return index;
  }
  index = first_free_peer_slot();
  if (index >= 0) {
    peers[index].label = label;
  }
  return index;
}

static String parse_sender_label(const String& text) {
  const int says_pos = text.indexOf(" says ");
  if (says_pos > 0) {
    return text.substring(0, says_pos);
  }

  const int from_pos = text.indexOf("from ");
  if (from_pos < 0) {
    return "";
  }
  const int start = from_pos + 5;
  int end = text.indexOf(' ', start);
  if (end < 0) {
    end = text.length();
  }
  return text.substring(start, end);
}

static String parse_recipient_label(const String& text) {
  const int says_hi_to_pos = text.indexOf(" says Hi to ");
  if (says_hi_to_pos >= 0) {
    const int start = says_hi_to_pos + 12;
    int end = text.indexOf(' ', start);
    if (end < 0) {
      end = text.length();
    }
    return text.substring(start, end);
  }

  const int to_pos = text.indexOf("to=");
  if (to_pos < 0) {
    return "";
  }
  const int start = to_pos + 3;
  int end = text.indexOf(' ', start);
  if (end < 0) {
    end = text.length();
  }
  return text.substring(start, end);
}

static void attach_link_callbacks(RNS::Link& link) {
  link.set_packet_callback(on_link_packet);
  link.set_link_closed_callback(on_link_closed);
}

static void on_link_packet(const RNS::Bytes& data, const RNS::Packet& packet) {
  String peer = "(unknown)";
  int peer_index = -1;
  if (packet.link()) {
    peer_index = find_peer_by_link_hash(packet.link().hash());
  }
  String text = data.toString().c_str();
  String sender = parse_sender_label(text);
  String recipient = parse_recipient_label(text);
  const String sender_label = peer_label_from_name(sender);
  if (is_local_name(sender) || (recipient.length() > 0 && !is_local_name(recipient))) {
    return;
  }
  if (peer_index >= 0 && peers[peer_index].label.length() == 0 &&
      sender.length() > 0 && !is_local_name(sender)) {
    const int existing_index = find_peer_by_label(sender_label);
    if (existing_index >= 0 && existing_index != peer_index) {
      if (packet.link()) {
        peers[existing_index].inbound_link = packet.link();
        peers[existing_index].inbound_active = true;
        attach_link_callbacks(peers[existing_index].inbound_link);
      }
      clear_peer_slot((uint8_t)peer_index);
      peer_index = existing_index;
    } else {
      peers[peer_index].label = sender_label;
    }
  }
  if (peer_index < 0) {
    if (sender.length() > 0 && !is_local_name(sender)) {
      peer_index = ensure_peer_for_label(sender_label);
      if (peer_index >= 0 && packet.link()) {
        peers[peer_index].inbound_link = packet.link();
        peers[peer_index].inbound_active = true;
        attach_link_callbacks(peers[peer_index].inbound_link);
      }
    }
  }
  if (peer_index >= 0) {
    peer = peers[peer_index].label;
    peers[peer_index].last_rx_ms = millis();
  }
  float rssi = lora_impl ? lora_impl->last_rssi() : 0.0f;
  float snr = lora_impl ? lora_impl->last_snr() : 0.0f;
  uint8_t physical_tx = lora_impl ? lora_impl->last_physical_tx() : 255;
  Serial.printf("RX LINK: %s | phy=%s(%u) RSSI=%.1f SNR=%.1f\r\n",
                text.c_str(),
                node_label_for_slot(physical_tx),
                (unsigned)physical_tx,
                rssi,
                snr);
  show_status("RX LINK", peer.c_str(), text.c_str());
}

static void on_link_closed(RNS::Link& link) {
  if (!link) {
    Serial.println("LINK CLOSED: null link callback");
    show_status("LINK CLOSED", "null");
    return;
  }

  const RNS::Bytes link_hash = link.hash();
  const String link_hash_hex = link_hash.toHex().c_str();
  int peer_index = find_peer_by_link_hash(link_hash);
  const char* label = "(unknown)";
  if (peer_index >= 0) {
    label = peers[peer_index].label.c_str();
    if (peers[peer_index].outbound_link && peers[peer_index].outbound_link.hash() == link_hash) {
      peers[peer_index].outbound_link = {RNS::Type::NONE};
      peers[peer_index].outbound_active = false;
      peers[peer_index].outbound_attempted = false;
      peers[peer_index].last_link_attempt_ms = 0;
      peers[peer_index].last_link_active_ms = 0;
      peers[peer_index].next_link_open_ms = millis() + LINK_REOPEN_DELAY_MS;
    }
    if (peers[peer_index].inbound_link && peers[peer_index].inbound_link.hash() == link_hash) {
      peers[peer_index].inbound_link = {RNS::Type::NONE};
      peers[peer_index].inbound_active = false;
      peers[peer_index].last_link_active_ms = 0;
    }
  }
  Serial.printf("LINK CLOSED: %s hash=%s\r\n", label, link_hash_hex.c_str());
  show_status("LINK CLOSED", label);
}

static void on_outbound_link_established(RNS::Link& link) {
  int peer_index = find_peer_by_link_hash(link.hash());
  if (peer_index >= 0) {
    peers[peer_index].outbound_link = link;
    attach_link_callbacks(peers[peer_index].outbound_link);
    peers[peer_index].outbound_active = true;
    peers[peer_index].outbound_attempted = true;
    peers[peer_index].outbound_failed = false;
    peers[peer_index].outbound_attempts = 0;
    peers[peer_index].outbound_attempt_window_ms = 0;
    peers[peer_index].last_link_attempt_ms = 0;
    peers[peer_index].last_link_active_ms = millis();
    Serial.printf("LINK ACTIVE: initiator link established to %s hash=%s\r\n",
                  peers[peer_index].label.c_str(), link.hash().toHex().c_str());
    show_status("LINK ACTIVE", peers[peer_index].label.c_str(), "initiator");
  } else {
    Serial.printf("LINK ACTIVE: initiator link established hash=%s peer=unknown\r\n",
                  link.hash().toHex().c_str());
  }
}

static void on_inbound_link_established(RNS::Link& link) {
  int peer_index = find_peer_by_link_hash(link.hash());
  if (peer_index < 0) {
    peer_index = first_free_peer_slot();
  }
  if (peer_index >= 0) {
    peers[peer_index].inbound_link = link;
    attach_link_callbacks(peers[peer_index].inbound_link);
    peers[peer_index].inbound_active = true;
    peers[peer_index].last_link_active_ms = millis();
  }
  uint8_t physical_tx = lora_impl ? lora_impl->last_physical_tx() : 255;
  Serial.printf("RX LINK: inbound link established hash=%s phy=%s(%u)\r\n",
                link.hash().toHex().c_str(),
                node_label_for_slot(physical_tx),
                (unsigned)physical_tx);
  show_status("LINK ACTIVE", "inbound", link.hash().toHex().c_str());
}

class LinkAnnounceHandler : public RNS::AnnounceHandler {
 public:
  LinkAnnounceHandler() : RNS::AnnounceHandler(ANNOUNCE_FILTER) {}

  void received_announce(const RNS::Bytes& destination_hash,
                         const RNS::Identity& announced_identity,
                         const RNS::Bytes& app_data) override {
    String label = app_data ? String(app_data.toString().c_str()) : String("(no label)");
    if (label == NODE_LABEL) {
      return;
    }
    if (!announced_identity) {
      Serial.printf("RX ANNOUNCE ignored: missing identity for hash=%s\r\n",
                    destination_hash.toHex().c_str());
      return;
    }

    int peer_index = ensure_peer_for_label(label);
    if (peer_index < 0) {
      Serial.printf("RX ANNOUNCE ignored: peer table full label=%s hash=%s\r\n",
                    label.c_str(), destination_hash.toHex().c_str());
      return;
    }

    peers[peer_index].destination_hash = destination_hash;
    peers[peer_index].destination = RNS::Destination(announced_identity,
                                                    RNS::Type::Destination::OUT,
                                                    RNS::Type::Destination::SINGLE,
                                                    destination_hash);
    peers[peer_index].announced = true;
    if (peers[peer_index].outbound_failed) {
      peers[peer_index].outbound_failed = false;
      peers[peer_index].outbound_attempts = 0;
      peers[peer_index].outbound_attempt_window_ms = 0;
      peers[peer_index].outbound_attempted = false;
      peers[peer_index].last_link_attempt_ms = 0;
      peers[peer_index].next_link_open_ms = millis() + LINK_REOPEN_DELAY_MS;
      Serial.printf("LINK RETRY RESET: fresh announce from %s\r\n", peers[peer_index].label.c_str());
    }

    uint8_t physical_tx = lora_impl ? lora_impl->last_physical_tx() : 255;
    Serial.printf("RX ANNOUNCE: label=%s hash=%s phy=%s(%u)\r\n",
                  peers[peer_index].label.c_str(),
                  peers[peer_index].destination_hash.toHex().c_str(),
                  node_label_for_slot(physical_tx),
                  (unsigned)physical_tx);
    show_status("RX ANNOUNCE", peers[peer_index].label.c_str(), peers[peer_index].destination_hash.toHex().c_str());
  }
};

static RNS::HAnnounceHandler announce_handler(new LinkAnnounceHandler());

static void print_config() {
  Serial.printf("Node=%s Board=%s Build=%s\r\n", NODE_LABEL, BOARD_ID, FW_BUILD_UTC);
  Serial.printf("Pins: CS=%d DIO1=%d RST=%d BUSY=%d SCK=%d MISO=%d MOSI=%d\r\n",
                (int)LORA_CS, (int)LORA_DIO1, (int)LORA_RESET, (int)LORA_BUSY,
                (int)LORA_SCK, (int)LORA_MISO, (int)LORA_MOSI);
  Serial.printf("LoRa: freq=%.1f BW=%.1f SF=%d CR=%d sync=0x%02x txp=%d\r\n",
                (double)LORA_FREQ_MHZ, (double)LORA_BW_KHZ, (int)LORA_SF,
                (int)LORA_CR, (int)LORA_SYNC_WORD, (int)LORA_TX_POWER_DBM);
  Serial.printf("Sim: phy_envelope=%d phy_block_bob_cy=%d node_slot=%d rns_log=warning linkfwd_delay_ms=%u transport=1\r\n",
                (int)SIM_PHY_ENVELOPE,
                (int)SIM_PHY_BLOCK_BOB_CY,
                (int)NODE_SLOT_INDEX,
                (unsigned)MR_LINKFWD_DELAY_MS);
  Serial.printf("Announce: startup=1 second=%lu repeat=%lu seconds\r\n",
                (unsigned long)ANNOUNCEMENT_2,
                (unsigned long)ANNOUNCEMENT_REPEAT);
}

static void send_announce() {
  if (!inbound_destination || !clock_ready) {
    return;
  }
  Serial.printf("TX ANNOUNCE: %s\r\n", NODE_LABEL);
  show_status("TX ANNOUNCE", NODE_LABEL);
  inbound_destination.announce(RNS::bytesFromString(NODE_LABEL));
}

static void maybe_send_scheduled_announce() {
  if (!clock_ready || !inbound_destination) {
    return;
  }

  static bool startup_announce_sent = false;
  static uint8_t announce_count = 0;
  static uint32_t next_announce_ms = 0;

  const uint32_t now = millis();
  if (!startup_announce_sent) {
    startup_announce_sent = true;
    announce_count = 1;
    next_announce_ms = now + ANNOUNCEMENT_2_MS;
    send_announce();
    return;
  }

  if ((int32_t)(now - next_announce_ms) < 0) {
    return;
  }

  send_announce();
  if (announce_count < 2) {
    announce_count = 2;
  }
  next_announce_ms = now + ANNOUNCEMENT_REPEAT_MS;
}

static void maybe_open_link(const DateTime& rtc_now, bool have_rtc_now) {
  const uint32_t now = millis();
  static uint32_t last_open_ms = 0;
  if (!clock_ready) {
    return;
  }
  for (uint8_t i = 0; i < MAX_PEERS; ++i) {
    PeerState& peer = peers[i];
    if (!peer.announced || !peer.destination) {
      continue;
    }
    if (peer.outbound_failed) {
      continue;
    }
    uint32_t stale_reference_ms = peer.last_rx_ms;
    if (stale_reference_ms == 0) {
      stale_reference_ms = peer.last_tx_ms;
    }
    if (stale_reference_ms == 0) {
      stale_reference_ms = peer.last_link_active_ms;
    }
    if (stale_reference_ms == 0) {
      stale_reference_ms = peer.last_link_attempt_ms;
    }
    if ((peer.outbound_link || peer.inbound_link) && stale_reference_ms != 0 &&
        (uint32_t)(now - stale_reference_ms) >= LINK_RX_STALE_MS) {
      Serial.printf("LINK STALE: no RX from %s after %lu ms ref=%lu; clearing link state\r\n",
                    peer.label.c_str(),
                    (unsigned long)LINK_RX_STALE_MS,
                    (unsigned long)stale_reference_ms);
      peer.outbound_link = {RNS::Type::NONE};
      peer.inbound_link = {RNS::Type::NONE};
      peer.outbound_active = false;
      peer.inbound_active = false;
      peer.outbound_attempted = false;
      peer.last_link_attempt_ms = 0;
      peer.last_link_active_ms = 0;
      peer.last_rx_ms = 0;
      peer.last_tx_ms = 0;
      peer.next_link_open_ms = now + LINK_REOPEN_DELAY_MS;
    }
    if (peer.next_link_open_ms != 0 && (int32_t)(now - peer.next_link_open_ms) < 0) {
      continue;
    }
    if (peer.outbound_active || (peer.outbound_link && peer.outbound_link.status() == RNS::Type::Link::ACTIVE)) {
      peer.outbound_active = true;
      continue;
    }
    if (peer.outbound_attempted && peer.last_link_attempt_ms != 0 &&
        (uint32_t)(now - peer.last_link_attempt_ms) >= LINK_RETRY_MS) {
      Serial.printf("LINK RETRY: no establishment after %lu ms; retrying %s attempts=%u/%u\r\n",
                    (unsigned long)LINK_RETRY_MS,
                    peer.label.c_str(),
                    (unsigned)peer.outbound_attempts,
                    (unsigned)LINK_MAX_ATTEMPTS_PER_WINDOW);
      peer.outbound_link = {RNS::Type::NONE};
      peer.outbound_attempted = false;
      peer.last_link_attempt_ms = 0;
      if (peer.outbound_attempts >= LINK_MAX_ATTEMPTS_PER_WINDOW &&
          peer.outbound_attempt_window_ms != 0 &&
          (uint32_t)(now - peer.outbound_attempt_window_ms) <= LINK_RETRY_WINDOW_MS) {
        peer.outbound_failed = true;
        Serial.printf("LINK FAILED: peer=%s attempts=%u window_ms=%lu waiting_for_announce=1\r\n",
                      peer.label.c_str(),
                      (unsigned)peer.outbound_attempts,
                      (unsigned long)(now - peer.outbound_attempt_window_ms));
        show_status("LINK FAILED", peer.label.c_str(), "wait announce");
        continue;
      }
    }
    if (peer.outbound_attempted || peer.outbound_link) {
      continue;
    }
    const uint8_t open_second = directed_link_open_second(peer.label);
    if (!have_rtc_now || rtc_now.second != open_second) {
      continue;
    }
    if (last_open_ms != 0 && (uint32_t)(now - last_open_ms) < 1500U) {
      return;
    }
    if (peer.outbound_attempt_window_ms == 0 ||
        (uint32_t)(now - peer.outbound_attempt_window_ms) >= LINK_RETRY_WINDOW_MS) {
      peer.outbound_attempt_window_ms = now;
      peer.outbound_attempts = 0;
    }
    if (peer.outbound_attempts >= LINK_MAX_ATTEMPTS_PER_WINDOW) {
      peer.outbound_failed = true;
      Serial.printf("LINK FAILED: peer=%s attempts=%u window_ms=%lu waiting_for_announce=1\r\n",
                    peer.label.c_str(),
                    (unsigned)peer.outbound_attempts,
                    (unsigned long)(now - peer.outbound_attempt_window_ms));
      show_status("LINK FAILED", peer.label.c_str(), "wait announce");
      continue;
    }

    ++peer.outbound_attempts;
    Serial.printf("TX LINKREQUEST: opening link to %s slot=%u attempt=%u/%u\r\n",
                  peer.label.c_str(),
                  (unsigned)open_second,
                  (unsigned)peer.outbound_attempts,
                  (unsigned)LINK_MAX_ATTEMPTS_PER_WINDOW);
    show_status("TX LINKREQ", peer.label.c_str());
    peer.outbound_link = RNS::Link(peer.destination);
    peer.outbound_link.set_packet_callback(on_link_packet);
    peer.outbound_link.set_link_established_callback(on_outbound_link_established);
    peer.outbound_link.set_link_closed_callback(on_link_closed);
    peer.outbound_attempted = true;
    peer.last_link_attempt_ms = now;
    last_open_ms = now;
    return;
  }
}

static void setup_reticulum() {
  microStore::FileSystem filesystem{microStore::Adapters::UniversalFileSystem()};
  filesystem.init();
  RNS::Utilities::OS::register_filesystem(filesystem);

  lora_impl = new TBeamSupremeLoRaInterface();
  lora_interface = lora_impl;
  lora_interface.mode(RNS::Type::Interface::MODE_GATEWAY);
  RNS::Transport::register_interface(lora_interface);
  lora_interface.start();

  reticulum = RNS::Reticulum();
  reticulum.transport_enabled(true);
  reticulum.probe_destination_enabled(false);

  RNS::Bytes private_key;
  if (!identity_bytes_from_hex(private_key)) {
    Serial.println("FATAL: identity hex decode failed");
    show_status("IDENTITY FAIL", BOARD_ID);
    while (true) {
      delay(1000);
    }
  }
  local_identity = RNS::Identity(false);
  if (!local_identity.load_private_key(private_key)) {
    Serial.println("FATAL: identity load failed");
    show_status("IDENTITY FAIL", BOARD_ID);
    while (true) {
      delay(1000);
    }
  }
  transport_identity = RNS::Identity(false);
  transport_identity.load_private_key(private_key);
  RNS::Transport::identity(transport_identity);

  reticulum.start();

  inbound_destination = RNS::Destination(local_identity,
                                         RNS::Type::Destination::IN,
                                         RNS::Type::Destination::SINGLE,
                                         APP_NAME,
                                         APP_ASPECT);
  inbound_destination.set_link_established_callback(on_inbound_link_established);
  inbound_destination.set_proof_strategy(RNS::Type::Destination::PROVE_NONE);

  RNS::Transport::register_announce_handler(announce_handler);

  Serial.printf("Local Identity: %s\r\n", local_identity.hash().toHex().c_str());
  Serial.printf("Local SINGLE destination: %s\r\n",
                inbound_destination.hash().toHex().c_str());
}

void setup() {
  Serial.begin(115200);
  while (!Serial && millis() < 5000) {
    delay(100);
  }
  delay(250);

  Serial.println();
  Serial.println("Exercise 205: sustained transport Links");
  RNS::loglevel(RNS::LOG_WARNING);

  (void)tbeam_supreme::initPmuForPeripherals(pmu, &Serial);
  tbeam::DisplayConfig display_config;
  display_config.powerSave = false;
  oled_display.begin(display_config);
  show_splash();
  delay(10000);

  print_config();
  mount_sd();

  if (saved_clock_is_fresh()) {
    clock_ready = true;
  } else {
    discipline_clock_from_gps();
    clock_ready = true;
  }

  show_status("Starting LoRa", NODE_LABEL);
  setup_reticulum();
  show_status("microR ready", NODE_LABEL, local_identity.hash().toHex().c_str());
  Serial.println("microReticulum ready");
}

void loop() {
  reticulum.loop();

  static uint32_t last_rtc_poll_ms = 0;
  static DateTime rtc_now{};
  static bool have_rtc_now = false;
  uint32_t now = millis();

  if ((uint32_t)(now - last_rtc_poll_ms) >= 200U) {
    last_rtc_poll_ms = now;
    have_rtc_now = read_rtc(rtc_now);
  }
  maybe_open_link(rtc_now, have_rtc_now);

  maybe_send_scheduled_announce();
  const uint8_t send_slot = node_send_slot_second();
  if (have_rtc_now && rtc_now.second == send_slot) {
    for (uint8_t i = 0; i < MAX_PEERS; ++i) {
      PeerState& peer = peers[i];
      if (peer.label.length() == 0 || peer.last_tx_second == rtc_now.second) {
        continue;
      }

      RNS::Link* link = nullptr;
      if (peer.outbound_link && peer.outbound_link.status() == RNS::Type::Link::ACTIVE) {
        peer.outbound_active = true;
        link = &peer.outbound_link;
      } else if (peer.inbound_link && peer.inbound_link.status() == RNS::Type::Link::ACTIVE) {
        peer.inbound_active = true;
        link = &peer.inbound_link;
      }
      if (!link) {
        continue;
      }

      peer.last_tx_second = rtc_now.second;
      peer.last_tx_ms = now;
      const char* recipient = board_id_for_label(peer.label);
      String message = String(BOARD_ID) + " says Hi to " + recipient + " iter=" + String(peer.tx_iter++);
      Serial.printf("TX LINK: %s via=%s hash=%s status=%u\r\n",
                    message.c_str(),
                    link == &peer.outbound_link ? "outbound" : "inbound",
                    link->hash().toHex().c_str(),
                    (unsigned)link->status());
      show_status("TX LINK", peer.label.c_str(), message.c_str());
      RNS::Packet(*link, RNS::bytesFromString(message.c_str())).send();
      delay(120);
    }
  }

  delay(5);
}

int _write(int file, char* ptr, int len) {
  (void)file;
  int wrote = Serial.write(ptr, len);
  Serial.flush();
  return wrote;
}