/***********************************************************************
Unified GPS & Magnetometer System
Main Control Program
ESP32-S3

MAINTENANCE / PORTING GOVERNANCE (USER-DIRECTED)
- This file is currently in an active port-and-stabilization phase.
- Keep behavior-preserving edits as the default.
- Use explicit, high-detail comments for non-obvious logic so future edits
    do not accidentally drop required functionality.
- Before removing or collapsing any existing function, discuss first and
    wait for user direction. Function removal is not a default refactor step.
- During replay/simulation work, prefer additive changes (flags, helpers,
    guarded branches) over destructive rewrites.

CURRENT IMPLEMENTATION SUMMARY
- Supports live serial capture mode and replay simulation mode.
- Replay mode reads full-length GPS/MAG data from SD-backed text files,
    derives source cadence from recorded timestamps, then applies replay
    speed scaling with bounded runtime periods.
- Display REC indicators are routed through a unified recording-state
    accessor so simulation and live behavior can be controlled consistently.
- Existing display, logging, alarm, and button pipelines are retained.
- Display modules remain compiled in simulator builds; headless mode only
    skips TFT runtime bring-up while display logic remains available.

This program integrates:

1. Magnetometer data acquisition - Magnetometer input + parsing + moving average + live graph (Display1_Mag)
2. GPS navigation data - GPS input + parsing (TinyGPS++) + moving map (Display2_Map)
3. SD card raw logging - Serial FIFO Buffer, log toggle via start/stop/restart
4. Moving-average baseline engine
5. Survey track storage
6. Real-time display system
7. WiFi Access Point (5 minutes) for file download/delete
8  4 displays: Mag Display, Map, GPS Comm Menu, Mag Comm Menu
9  4 Buttons: Cycle display -> Button1, Menu Down -> Button2, Menu Up -> Button3, Menu Select -> Button4
10. Read serial operator commands (start/stop/restart recording) on USB Serial0

All RAW serial data must be logged EXACTLY as received.
No processing is permitted to alter recorded sensor data.

All RAW serial data must be logged EXACTLY as received.
No processing is permitted to alter recorded sensor data.
***********************************************************************/

#include <Arduino.h>

#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <SPI.h>

// Wokwi / ESP32-S3 display wiring used by the probe-proven Adafruit_ILI9341 path.
// These values are taken from the active diagram.json in this workspace so the
// driver object matches the current simulator wiring exactly.
//
// IMPORTANT:
// - Wokwi TFT CS = GPIO15
// - Wokwi TFT DC = GPIO2
// - SD card CS  = GPIO46 (kept separately below as SD_CS)
//
// Real-hardware wiring can still be reintroduced later behind an explicit build
// flag, but Wokwi validation should follow the simulator diagram first.
#define TFT_MOSI 11
#define TFT_MISO 13
#define TFT_SCLK 12
#define TFT_CS 15
#define TFT_DC 2
#define TFT_RST 21
#define TFT_BL 14

Adafruit_ILI9341 tft(TFT_CS, TFT_DC, TFT_RST);

// The Wokwi display runtime is now intended to be active again because the
// validated probe path uses Adafruit_ILI9341 instead of TFT_eSPI.
#define HEADLESS_WOKWI 0

// Controlled next-step display bring-up:
// In headless Wokwi mode we can still probe low-level TFT transport safely
// without enabling the full sprite/display stack. This helps isolate
// whether failures come from SPI/TFT init or from later rendering layers.
// Result from the earlier TFT_eSPI investigation:
// - SPI.begin() succeeded
// - TFT_eSPI tft.begin() crashed with StoreProhibited in the current Wokwi setup
// That investigation is retained here as a note, but the main Wokwi path now
// uses Adafruit_ILI9341 instead, so this probe stays disabled.
#define ENABLE_MINIMAL_TFT_BRINGUP_PROBE 0

// Replay simulation remains enabled independently of display bring-up so the
// SD-backed validation data set can drive the live display path in Wokwi.
#define ENABLE_FILE_REPLAY_SIM 1

#define ENABLE_DISPLAY_STACK 1

/***********************************************************************
SYSTEM MODULES
***********************************************************************/

#include "SDLogger.h"
#include "MagParser.h"
#include "magMovingAverage.h"

#include "GPSHandler.h"
#include "GPSConfig.h"
#include "GPSDistance.h"

#include "PathFIFO.h"
#include "SerialFIFOBuffer.h"

#include "RuntimeParams.h"

#if ENABLE_DISPLAY_STACK
#include "Display1_Mag.h"
#include "Display2_Map.h"
#include "Display3_GPSCom.h"
#include "Display4_MagCom.h"
#include "Display5_Params.h"
#endif

#include "WebServer.h"
#include "Mag_WebServer.h"
#include "MagAlarmUtils.h"

/***********************************************************************
IST8310 COMPASS DRIVER
***********************************************************************/

#include "IST8310.h"

/***********************************************************************
SYSTEM SERIAL PORT ASSIGNMENTS
***********************************************************************/

/*
Serial0 → USB Debug / Operator Commands

Serial1 → Marine Magnetometer
          UART1
          RX = GPIO4
          TX = GPIO5
*/
#define MAG_SERIAL Serial1
#define MAG_BAUD   9600
#define MAG_RX_PIN 4
#define MAG_TX_PIN 5

#define GPS_SERIAL Serial2
#define GPS_BAUD 9600
#define GPS_RX_PIN 16
#define GPS_TX_PIN 17

// Passive buzzer (KY-012) on ESP32-S3
#define BUZZER_PIN 43      // using available pin 43 (11/12/13 already in use)
#define BUZZER_CHANNEL 0   // LEDC PWM channel for tone output
#define SD_CS 46

#if ENABLE_FILE_REPLAY_SIM
#define REPLAY_GPS_FILE_PATH "/replay_gps_2023-08-09.txt"
#define REPLAY_MAG_FILE_PATH "/replay_mag_2023-08-09.txt"
#endif



// ===================== WIFI http://192.168.4.1=====================
WebServer server(80);
unsigned long wifiStartTime = 0;
bool wifiActive = false;


/***********************************************************************
Read the compass only twice per second
***********************************************************************/
unsigned long compassTimer = 0;
const unsigned long COMPASS_INTERVAL = 500;   // 500 ms = 2 Hz


/***********************************************************************
MAGNETOMETER MOVING AVERAGE ENGINE
***********************************************************************/

// Moving-average engine
MagMovingAverage magAvg;

// Current moving-average window size (runtime configurable)
int currentAverageWindow = 0;

// Baseline magnetic field
float magAverage = 0;

// Deviation from baseline
float magDeviation = 0;

// Alarm state indicator - true when alarm tone is active
bool alarmActive = false;

// Number of magnetometer samples received
int magSamples = 0;

// Time-based averaging/history policy.
// Moving-average windows are now expressed as duration targets, not fixed
// sample counts: short=4s, long=2m. Sample count is derived from measured or
// derived source cadence so behavior stays consistent at different rates.
const int MOVING_AVG_SHORT_SECONDS_MAIN = 4;
const int MOVING_AVG_LONG_SECONDS_MAIN = 120;
const unsigned long DEFAULT_MAG_SOURCE_PERIOD_MS_MAIN = 250;
const unsigned long DEFAULT_GPS_SOURCE_PERIOD_MS_MAIN = 1000;

// Display2 map history target: approximately 30 minutes of path data.
const int MAP_HISTORY_TARGET_SECONDS_MAIN = 1800;
const int MAP_HISTORY_MIN_POINTS_MAIN = 300;
const int MAP_HISTORY_MAX_POINTS_MAIN = 7200;

unsigned long magSourcePeriodMsMain = DEFAULT_MAG_SOURCE_PERIOD_MS_MAIN;
unsigned long gpsSourcePeriodMsMain = DEFAULT_GPS_SOURCE_PERIOD_MS_MAIN;

unsigned long lastMovingAverageRecalcMsMain = 0;
const unsigned long MOVING_AVG_RECALC_INTERVAL_MS_MAIN = 1500;

// Runtime-adjustable settings (MVP scope for Display5 parameter screen).
int gMovingAverageMenuSizeMain = 480;
float gDeviationLowThresholdMain = 2.0f;
float gDeviationHighThresholdMain = 4.0f;
bool gAlarmEnabledMain = true;
AlarmTriggerModeMain gAlarmTriggerModeMain = ALARM_TRIGGER_LOW_MAIN;


/***********************************************************************
COMPASS STATE VARIABLES
These are made global so displays can access them
***********************************************************************/

IST8310 compass;

float compassHeading = 0.0;
bool compassCalibrationRequired = true;


/***********************************************************************
SYSTEM OBJECTS
***********************************************************************/

SDLogger logger;

MagParser magParser;

GPSHandler gpsHandler;

GPSDistance gpsDistance;

PathFIFO gpsPath;

#if ENABLE_DISPLAY_STACK
Display1_Mag displayMag;
Display2_Map displayMap;
Display3_GPSCom displayGPSCom;
Display4_MagCom displayMagCom;
Display5_Params displayParams;
#endif

// Interrupt-driven button handling (probe-compatible)
// Button pins
static const int BUTTON_PIN_1 = 42;
static const int BUTTON_PIN_2 = 41;
static const int BUTTON_PIN_3 = 40;
static const int BUTTON_PIN_4 = 39;
static const int BUTTON_PINS[4] = {BUTTON_PIN_1, BUTTON_PIN_2, BUTTON_PIN_3, BUTTON_PIN_4};

// Button state tracking
volatile int pendingButtonIndex = -1;
static unsigned long buttonLastFired[4] = {0, 0, 0, 0};
const unsigned long BUTTON_COOLDOWN_MS = 300;


/***********************************************************************
SERIAL FIFO BUFFERS
***********************************************************************/

SerialFIFOBuffer magFIFO;
SerialFIFOBuffer gpsFIFO;


/***********************************************************************
DISPLAY CONTROLLER
***********************************************************************/

enum DisplayMode
{
    DISPLAY_MAG = 0,
    DISPLAY_MAP = 1,
    DISPLAY_GPSCOM = 2,
    DISPLAY_MAGCOM = 3,
    DISPLAY_PARAMS = 4
};

DisplayMode currentDisplay = DISPLAY_MAG;
DisplayMode lastDisplay = DISPLAY_MAG;

// Probe-style render pacing keeps the UI responsive in Wokwi.
// Without pacing, full-screen redraws can saturate the simulator and make
// button/display interaction feel clunky even when data ingest is correct.
static unsigned long lastDisplayRenderMs = 0;
const unsigned long DISPLAY_RENDER_INTERVAL_MAG_MS = 40;
const unsigned long DISPLAY_RENDER_INTERVAL_MAP_MS = 60;
const unsigned long DISPLAY_RENDER_INTERVAL_GPSCOM_MS = 50;
const unsigned long DISPLAY_RENDER_INTERVAL_MAGCOM_MS = 50;
const unsigned long DISPLAY_RENDER_INTERVAL_PARAMS_MS = 70;


/***********************************************************************
SYSTEM STATE
***********************************************************************/

bool systemInitialized = false;
bool displayReady = false;
bool minimalTftBringupReady = false;

#if ENABLE_FILE_REPLAY_SIM
File gpsReplayFileMain;
File magReplayFileMain;
bool gpsReplayReadyMain = false;
bool magReplayReadyMain = false;
bool gpsReplayHasPendingLineMain = false;
String gpsReplayPendingLineMain;

unsigned long lastGpsReplayMs = 0;
unsigned long lastMagReplayMs = 0;

// Replay speed control for simulation.
// Supported values are intentionally limited to 2x, 4x, or 8x so replay
// timing remains predictable and easy to reason about during testing.
const unsigned long REPLAY_SPEED_MULTIPLIER_MAIN = 4;
unsigned long gpsReplayPeriodMsMain = 1000 / REPLAY_SPEED_MULTIPLIER_MAIN;
unsigned long magReplayPeriodMsMain = 500 / REPLAY_SPEED_MULTIPLIER_MAIN;

// Startup alignment between the first valid GPS timestamp and the first valid
// MAG timestamp found in the replay files. This is ONLY for simulation and is
// intentionally separate from live magnetometer clock-sync behavior.
//
// Meaning:
// - Positive value  => MAG source starts later than GPS source.
// - Negative value  => GPS source starts later than MAG source.
long replayStartOffsetSourceMsMain = 0;
unsigned long replayStartDelayGpsMsMain = 0;
unsigned long replayStartDelayMagMsMain = 0;

// Cache the first valid timestamps seen during cadence derivation.
// This lets startup alignment reuse the same early-file scan instead of
// reopening and rewalking long replay files a second time.
bool gpsReplayFirstTimeCachedMain = false;
long gpsReplayFirstTimeMsMain = 0;
bool magReplayFirstTimeCachedMain = false;
long magReplayFirstTimeMsMain = 0;

// Replay setup should stay responsive even when source files are very long.
// We therefore derive cadence from only a small early subset of valid data.
// These limits are intentionally conservative: enough samples to estimate
// source rate, but not enough to create a noticeable startup pause.
const int GPS_REPLAY_DERIVE_TARGET_TRANSITIONS_MAIN = 10;
const int GPS_REPLAY_DERIVE_MAX_LINES_MAIN = 64;
const int MAG_REPLAY_DERIVE_TARGET_DELTAS_MAIN = 16;
const int MAG_REPLAY_DERIVE_MAX_LINES_MAIN = 64;

const unsigned long MIN_REPLAY_PERIOD_MS_MAIN = 20;
const unsigned long MAX_REPLAY_PERIOD_MS_MAIN = 5000;
const uint8_t MAX_GPS_CATCHUP_STEPS_PER_LOOP_MAIN = 4;
const uint8_t MAX_MAG_CATCHUP_STEPS_PER_LOOP_MAIN = 8;

bool replaySimRecordingMain = true;
#endif

// Unified display-facing recording state.
// In replay mode we intentionally decouple REC state from SD writes so
// we can exercise full UI/menu behavior even when using simulated feeds.
bool isRecordingForDisplay()
{
#if ENABLE_FILE_REPLAY_SIM
    return replaySimRecordingMain;
#else
    return logger.isRecording();
#endif
}

float wrap360Main(float deg)
{
    while (deg < 0.0f)
        deg += 360.0f;

    while (deg >= 360.0f)
        deg -= 360.0f;

    return deg;
}

float shortestHeadingDeltaMain(float fromDeg, float toDeg)
{
    float delta = wrap360Main(toDeg) - wrap360Main(fromDeg);

    if (delta > 180.0f)
        delta -= 360.0f;
    else if (delta < -180.0f)
        delta += 360.0f;

    return delta;
}

float clampfMain(float value, float lo, float hi)
{
    if (value < lo)
        return lo;

    if (value > hi)
        return hi;

    return value;
}

unsigned long clampSourcePeriodMsMain(unsigned long periodMs, unsigned long minMs, unsigned long maxMs)
{
    if (periodMs < minMs)
        return minMs;

    if (periodMs > maxMs)
        return maxMs;

    return periodMs;
}

int movingAverageDurationSecondsFromMenuMain(int menuSize)
{
    // Preserve existing menu semantics:
    // - 4   => short window
    // - 480 => long window (legacy 4Hz*120s), now interpreted as 2 minutes.
    if (menuSize <= 4)
        return MOVING_AVG_SHORT_SECONDS_MAIN;

    return MOVING_AVG_LONG_SECONDS_MAIN;
}

int computeMovingAverageSamplesMain(int durationSeconds, unsigned long sourcePeriodMs)
{
    const unsigned long clampedPeriodMs = clampSourcePeriodMsMain(sourcePeriodMs, 20, 5000);
    unsigned long samples = ((unsigned long)durationSeconds * 1000UL + (clampedPeriodMs / 2UL)) / clampedPeriodMs;

    if (samples < 1UL)
        samples = 1UL;

    if (samples > 10000UL)
        samples = 10000UL;

    return (int)samples;
}

int computeMapPathCapacityMain(unsigned long gpsPeriodMs)
{
    // Target a roughly fixed 30-minute lookback on Display2 map, then clamp
    // for RAM safety so fast GPS rates do not exhaust memory.
    const unsigned long clampedPeriodMs = clampSourcePeriodMsMain(gpsPeriodMs, 100, 10000);
    unsigned long samples = ((unsigned long)MAP_HISTORY_TARGET_SECONDS_MAIN * 1000UL + (clampedPeriodMs / 2UL)) / clampedPeriodMs;

    if ((int)samples < MAP_HISTORY_MIN_POINTS_MAIN)
        samples = (unsigned long)MAP_HISTORY_MIN_POINTS_MAIN;

    if ((int)samples > MAP_HISTORY_MAX_POINTS_MAIN)
        samples = (unsigned long)MAP_HISTORY_MAX_POINTS_MAIN;

    return (int)samples;
}

#if !ENABLE_DISPLAY_STACK
int getHeadlessDefaultMovingAverageSize()
{
    return 480;
}
#endif

#if ENABLE_FILE_REPLAY_SIM
unsigned long clampReplayPeriodMain(unsigned long periodMs);
unsigned long normalizeReplaySpeedMultiplierMain(unsigned long multiplier);
bool openReplayFileMain(const char* path, File& replayFile);
bool rewindReplayFileMain(const char* path, File& replayFile);
bool readNextReplayLineMain(File& replayFile, const char* path, String& line);
unsigned long deriveGpsCapturePeriodMsMain();
unsigned long deriveMagCapturePeriodMsMain();
bool extractFirstGpsTimeOfDayMsMain(long& timeMs);
bool extractFirstMagTimeOfDayMsMain(long& timeMs);
long computeReplayStartOffsetSourceMsMain();
#endif

/***********************************************************************
BUTTON ISR HANDLERS
***********************************************************************/

void IRAM_ATTR onButton1() {
    if (pendingButtonIndex < 0) {
        pendingButtonIndex = 0;
    }
}

void IRAM_ATTR onButton2() {
    if (pendingButtonIndex < 0) {
        pendingButtonIndex = 1;
    }
}

void IRAM_ATTR onButton3() {
    if (pendingButtonIndex < 0) {
        pendingButtonIndex = 2;
    }
}

void IRAM_ATTR onButton4() {
    if (pendingButtonIndex < 0) {
        pendingButtonIndex = 3;
    }
}

int consumePendingButtonIndex() {
    noInterrupts();
    int buttonIndex = pendingButtonIndex;
    pendingButtonIndex = -1;
    interrupts();
    return buttonIndex;
}


/***********************************************************************
UTILITY
Convert GPS Date to File Name
***********************************************************************/

String getDateString()
{
    int year  = gpsHandler.getYear();
    int month = gpsHandler.getMonth();
    int day   = gpsHandler.getDay();

    char buf[16];

    sprintf(buf,"%04d-%02d-%02d",year,month,day);

    return String(buf);
}


/***********************************************************************
SYSTEM SETUP
***********************************************************************/

void setup()
{
    Serial.begin(115200);

    delay(1000);

    Serial.println();
    Serial.println("Unified GPS & Magnetometer System Booting");
    Serial.println("CHK: setup start");

    /***************************************************************
    EARLY DISPLAY PROBE
    Initialize TFT before other subsystems to rule out prior memory corruption.
    ***************************************************************/
    #if HEADLESS_WOKWI
    #if ENABLE_MINIMAL_TFT_BRINGUP_PROBE
    Serial.println("CHK: TFT minimal probe start (HEADLESS_WOKWI)");
    Serial.println("CHK: TFT probe SPI.begin start");
    SPI.begin(TFT_SCLK, TFT_MISO, TFT_MOSI, TFT_CS);
    Serial.println("CHK: TFT probe SPI.begin done");

    pinMode(TFT_BL, OUTPUT);
    digitalWrite(TFT_BL, HIGH);
    Serial.println("CHK: TFT probe backlight set");

    Serial.println("CHK: TFT probe tft.begin start");
    tft.begin();
    Serial.println("CHK: TFT probe tft.begin done");

    tft.setRotation(1);
    Serial.println("CHK: TFT probe rotation done");
    tft.fillScreen(ILI9341_BLACK);
    Serial.println("CHK: TFT probe clear done");

    minimalTftBringupReady = true;
    displayReady = false;
    Serial.println("CHK: TFT probe passed; full display stack remains disabled in headless mode");
    #else
    Serial.println("CHK: TFT skipped (HEADLESS_WOKWI)");
    #endif
    displayReady = false;
    #else
    Serial.println("CHK: TFT pre-init ready");

    SPI.begin(TFT_SCLK, TFT_MISO, TFT_MOSI, TFT_CS);
    pinMode(TFT_BL, OUTPUT);
    digitalWrite(TFT_BL, HIGH);
    tft.begin();
    Serial.println("CHK: TFT begin done");
    tft.setRotation(1);
    tft.fillScreen(ILI9341_BLACK);
    Serial.println("CHK: TFT clear done");
    displayReady = true;
    #endif


    /***************************************************************
    SERIAL PORT INITIALIZATION
    ***************************************************************/

    MAG_SERIAL.begin(
        MAG_BAUD,
        SERIAL_8N1,
        MAG_RX_PIN,
        MAG_TX_PIN
    );
    Serial.println("CHK: MAG serial started");

    GPS_SERIAL.begin(
        GPS_BAUD,
        SERIAL_8N1,
        GPS_RX_PIN,
        GPS_TX_PIN
    );
    Serial.println("CHK: GPS serial started");
    configureGPS(GPS_SERIAL);
    Serial.println("CHK: GPS configured");

    MAG_SERIAL.setRxBufferSize(2048);
    GPS_SERIAL.setRxBufferSize(2048);
    Serial.println("CHK: RX buffers set");

    /***************************************************************
    DISPLAY SYSTEM INITIALIZATION
    ***************************************************************/

    /***************************************************************
    FIFO BUFFER INITIALIZATION
    ***************************************************************/

    magFIFO.begin();
    gpsFIFO.begin();
    Serial.println("CHK: FIFO init done");

    // Initialize map history capacity from expected GPS cadence.
    // Replay mode may refine this once source cadence is derived from file data.
    gpsPath.init(computeMapPathCapacityMain(gpsSourcePeriodMsMain));
    Serial.println("CHK: Path FIFO init done");


    /***************************************************************
    SD LOGGER INITIALIZATION
    ***************************************************************/

    if (!logger.begin(SD_CS))
    {
        Serial.println("SD Logger initialization failed");
    }
    Serial.println("CHK: SD logger init done");


    /***************************************************************
    DISPLAY SYSTEM INITIALIZATION
    ***************************************************************/

    if (displayReady)
    {
        #if ENABLE_DISPLAY_STACK
        displayMag.begin();
        Serial.println("CHK: displayMag begin done");
        displayMap.begin();
        Serial.println("CHK: displayMap begin done");
        displayGPSCom.begin();
        Serial.println("CHK: displayGPSCom begin done");
        displayMagCom.begin();
        Serial.println("CHK: displayMagCom begin done");
        displayParams.begin();
        Serial.println("CHK: displayParams begin done");

        // Keep D4 and D5 pointing at one shared moving-average mode.
        displayMagCom.setMovingAverageSize(gMovingAverageMenuSizeMain);
        displayParams.setMovingAverageSize(gMovingAverageMenuSizeMain);
        #endif
    }


    /***************************************************************
    BUTTON INITIALIZATION (Interrupt-driven)
    ***************************************************************/

    for (int i = 0; i < 4; ++i) {
        pinMode(BUTTON_PINS[i], INPUT_PULLUP);
    }

    attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_1), onButton1, FALLING);
    attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_2), onButton2, FALLING);
    attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_3), onButton3, FALLING);
    attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_4), onButton4, FALLING);

    Serial.println("CHK: buttons begin done");

    /***************************************************************
    BUZZER / ALARM INITIALIZATION
    ***************************************************************/

   // KY-012 passive buzzer: SIG -> BUZZER_PIN, GND -> GND, VCC -> 3.3V
    pinMode(BUZZER_PIN, OUTPUT);
    ledcAttach(BUZZER_PIN, 2000, 8);      // (Pin, Frequency in Hz, Resolution in bits)
    ledcWrite(BUZZER_PIN, 0);             // In Core 3.x, you use the PIN, not the channel
    Serial.println("CHK: buzzer init done");

    /***************************************************************
    INITIALIZE MOVING AVERAGE ENGINE
    ***************************************************************/

    #if ENABLE_DISPLAY_STACK
    {
        const int avgDurationSeconds = movingAverageDurationSecondsFromMenuMain(gMovingAverageMenuSizeMain);
        currentAverageWindow = computeMovingAverageSamplesMain(avgDurationSeconds, magSourcePeriodMsMain);
    }
    #else
    currentAverageWindow = getHeadlessDefaultMovingAverageSize();
    #endif
    magAvg.init(currentAverageWindow);
    Serial.println("CHK: moving average init done");

    /***************************************************************
    IST8310 COMPASS INITIALIZATION
    ***************************************************************/

    Wire.begin(9,10); // SDA = 9, SCL = 10 (M10 GPS internal compass)
    Serial.println("CHK: Wire begin done");

    if(compass.begin())
    {
        Serial.println("IST8310 Compass detected");
    }
    else
    {
        Serial.println("IST8310 Compass NOT detected");
    }


/***************************************************************
START WIFI ACCESS POINT http://192.168.4.1
***************************************************************/

WiFi.softAP(ssid, password);
wifiActive = true;
Serial.println("CHK: WiFi AP started");

    initWebServer(server);
Serial.println("CHK: web server init done");
wifiStartTime = millis();

Serial.println("WiFi Access Point Started");


systemInitialized = true;

#if ENABLE_FILE_REPLAY_SIM
unsigned long now = millis();
lastGpsReplayMs = now;
lastMagReplayMs = now;
Serial.println("CHK: replay setup enter");

gpsReplayReadyMain = openReplayFileMain(REPLAY_GPS_FILE_PATH, gpsReplayFileMain);
Serial.println("CHK: replay gps open attempted");
magReplayReadyMain = openReplayFileMain(REPLAY_MAG_FILE_PATH, magReplayFileMain);
Serial.println("CHK: replay mag open attempted");
gpsReplayHasPendingLineMain = false;

Serial.println("CHK: replay derive gps period start");
unsigned long gpsCapturePeriodMs = deriveGpsCapturePeriodMsMain();
Serial.println("CHK: replay derive gps period done");
Serial.println("CHK: replay derive mag period start");
unsigned long magCapturePeriodMs = deriveMagCapturePeriodMsMain();
Serial.println("CHK: replay derive mag period done");

// In replay mode we can use source-derived cadence immediately.
gpsSourcePeriodMsMain = clampSourcePeriodMsMain(gpsCapturePeriodMs, 100, 10000);
magSourcePeriodMsMain = clampSourcePeriodMsMain(magCapturePeriodMs, 20, 5000);

// Re-size map storage based on derived GPS source cadence so Display2 history
// is closer to a fixed 30-minute lookback at different replay rates.
gpsPath.init(computeMapPathCapacityMain(gpsSourcePeriodMsMain));

const unsigned long replaySpeedMultiplierMain = normalizeReplaySpeedMultiplierMain(REPLAY_SPEED_MULTIPLIER_MAIN);
gpsReplayPeriodMsMain = clampReplayPeriodMain(max(1UL, gpsCapturePeriodMs / replaySpeedMultiplierMain));
magReplayPeriodMsMain = clampReplayPeriodMain(max(1UL, magCapturePeriodMs / replaySpeedMultiplierMain));

#if ENABLE_DISPLAY_STACK
{
    const int avgDurationSeconds = movingAverageDurationSecondsFromMenuMain(gMovingAverageMenuSizeMain);
    const int desiredAverageSamples = computeMovingAverageSamplesMain(avgDurationSeconds, magSourcePeriodMsMain);
    if (desiredAverageSamples != currentAverageWindow)
    {
        currentAverageWindow = desiredAverageSamples;
        magAvg.init(currentAverageWindow);
    }
}
#endif

// Auto-align replay streams using first valid timestamps from the source files.
// This compensates for slight real-world logging skew between GPS and MAG
// recordings while preserving independent cadence derivation for each stream.
replayStartOffsetSourceMsMain = computeReplayStartOffsetSourceMsMain();
replayStartDelayGpsMsMain = 0;
replayStartDelayMagMsMain = 0;

if (replayStartOffsetSourceMsMain > 0)
    replayStartDelayMagMsMain = (unsigned long)((replayStartOffsetSourceMsMain + (long)(replaySpeedMultiplierMain / 2)) / (long)replaySpeedMultiplierMain);
else if (replayStartOffsetSourceMsMain < 0)
    replayStartDelayGpsMsMain = (unsigned long)(((-replayStartOffsetSourceMsMain) + (long)(replaySpeedMultiplierMain / 2)) / (long)replaySpeedMultiplierMain);

lastGpsReplayMs = now + gpsReplayPeriodMsMain - replayStartDelayGpsMsMain;
lastMagReplayMs = now + magReplayPeriodMsMain - replayStartDelayMagMsMain;

Serial.print("CHK: replay sim enabled x");
Serial.println(replaySpeedMultiplierMain);
Serial.print("CHK: replay gps file ready=");
Serial.println(gpsReplayReadyMain ? "YES" : "NO");
Serial.print("CHK: replay mag file ready=");
Serial.println(magReplayReadyMain ? "YES" : "NO");
Serial.print("CHK: replay source periods gps=");
Serial.print(gpsCapturePeriodMs);
Serial.print("ms mag=");
Serial.print(magCapturePeriodMs);
Serial.println("ms");
Serial.print("CHK: replay runtime periods gps=");
Serial.print(gpsReplayPeriodMsMain);
Serial.print("ms mag=");
Serial.print(magReplayPeriodMsMain);
Serial.println("ms");
Serial.print("CHK: replay start offset source=");
Serial.print(replayStartOffsetSourceMsMain);
Serial.println("ms");
Serial.print("CHK: replay start delay gps=");
Serial.print(replayStartDelayGpsMsMain);
Serial.print("ms mag=");
Serial.print(replayStartDelayMagMsMain);
Serial.println("ms");
Serial.print("CHK: sim recording state=");
Serial.println(replaySimRecordingMain ? "ON" : "OFF");
#endif

Serial.println("System Ready");

// Final screen clear before entering main loop to eliminate any startup artifacts.
// Then redraw the active display's static frame so chrome is immediately visible.
if (displayReady)
{
    tft.fillScreen(ILI9341_BLACK);
    #if ENABLE_DISPLAY_STACK
    switch(currentDisplay)
    {
        case DISPLAY_MAG:
            displayMag.begin();
            break;
        case DISPLAY_MAP:
            displayMap.begin();
            break;
        case DISPLAY_GPSCOM:
            displayGPSCom.begin();
            break;
        case DISPLAY_MAGCOM:
            displayMagCom.begin();
            break;
        case DISPLAY_PARAMS:
            displayParams.begin();
            break;
    }
    #endif
}
}

#if ENABLE_FILE_REPLAY_SIM
bool isDigitCharMain(char c)
{
    return c >= '0' && c <= '9';
}

String tokenAtMain(const String& input, int targetIndex)
{
    int currentIndex = 0;
    int start = 0;

    for (int i = 0; i <= input.length(); ++i)
    {
        if (i == input.length() || input[i] == ',')
        {
            if (currentIndex == targetIndex)
                return input.substring(start, i);

            currentIndex++;
            start = i + 1;
        }
    }

    return String();
}

String gpsEpochKeyFromLineMain(const String& line)
{
    String rawTime = tokenAtMain(line, 1);
    if (rawTime.length() < 6)
        return String();

    for (int i = 0; i < 6; ++i)
    {
        if (!isDigitCharMain(rawTime[i]))
            return String();
    }

    return rawTime.substring(0, 6);
}

unsigned long clampReplayPeriodMain(unsigned long periodMs)
{
    // Keep replay periods within safe bounds to prevent run-away loops
    // or near-frozen updates when parsed source data is unusual.
    if (periodMs < MIN_REPLAY_PERIOD_MS_MAIN)
        return MIN_REPLAY_PERIOD_MS_MAIN;

    if (periodMs > MAX_REPLAY_PERIOD_MS_MAIN)
        return MAX_REPLAY_PERIOD_MS_MAIN;

    return periodMs;
}

unsigned long normalizeReplaySpeedMultiplierMain(unsigned long multiplier)
{
    // Keep replay-speed selection intentionally simple for operator testing.
    // Any unsupported value falls back to the standard 4x mode.
    if (multiplier == 2UL || multiplier == 4UL || multiplier == 8UL)
        return multiplier;

    return 4UL;
}

bool openReplayFileMain(const char* path, File& replayFile)
{
    if (replayFile)
        replayFile.close();

    replayFile = SD.open(path, FILE_READ);
    if (!replayFile)
    {
        Serial.print("WARN: replay open failed ");
        Serial.println(path);
        return false;
    }

    return true;
}

bool rewindReplayFileMain(const char* path, File& replayFile)
{
    return openReplayFileMain(path, replayFile);
}

bool readNextReplayLineMain(File& replayFile, const char* path, String& line)
{
    if (!replayFile && !openReplayFileMain(path, replayFile))
        return false;

    while (replayFile.available())
    {
        line = replayFile.readStringUntil('\n');
        line.trim();

        if (line.length() > 0)
            return true;
    }

    return false;
}

bool readNextReplayLineNoWrapMain(File& replayFile, String& line)
{
    if (!replayFile)
        return false;

    while (replayFile.available())
    {
        line = replayFile.readStringUntil('\n');
        line.trim();

        if (line.length() > 0)
            return true;
    }

    return false;
}

bool parseHhmmssTokenToMsMain(const String& token, long& timeMs)
{
    // Accepts HHMMSS or HHMMSS.sss style tokens.
    // This is shared by both GPS and MAG cadence derivation helpers.
    if (token.length() < 6)
        return false;

    for (int i = 0; i < 6; ++i)
    {
        if (!isDigitCharMain(token[i]))
            return false;
    }

    int hh = (token[0] - '0') * 10 + (token[1] - '0');
    int mm = (token[2] - '0') * 10 + (token[3] - '0');
    int ss = (token[4] - '0') * 10 + (token[5] - '0');

    if (hh > 23 || mm > 59 || ss > 59)
        return false;

    int millisPart = 0;
    int millisDigits = 0;

    if (token.length() > 6 && token[6] == '.')
    {
        for (int i = 7; i < token.length() && millisDigits < 3; ++i)
        {
            if (!isDigitCharMain(token[i]))
                break;

            millisPart = millisPart * 10 + (token[i] - '0');
            millisDigits++;
        }

        while (millisDigits < 3)
        {
            millisPart *= 10;
            millisDigits++;
        }
    }

    timeMs = ((long)hh * 3600L + (long)mm * 60L + (long)ss) * 1000L + millisPart;
    return true;
}

bool extractTimeOfDayMsFromLineMain(const String& line, long& timeMs)
{
    // MAG replay lines are not guaranteed to be comma-token aligned,
    // so scan entire sentence for HH:MM:SS[.sss] pattern.
    int n = line.length();

    for (int i = 0; i + 7 < n; ++i)
    {
        bool patternMatch =
            isDigitCharMain(line[i]) &&
            isDigitCharMain(line[i + 1]) &&
            line[i + 2] == ':' &&
            isDigitCharMain(line[i + 3]) &&
            isDigitCharMain(line[i + 4]) &&
            line[i + 5] == ':' &&
            isDigitCharMain(line[i + 6]) &&
            isDigitCharMain(line[i + 7]);

        if (!patternMatch)
            continue;

        String token = line.substring(i, i + 8);

        int j = i + 8;
        if (j < n && line[j] == '.')
        {
            token += '.';
            j++;
            while (j < n && isDigitCharMain(line[j]))
            {
                token += line[j];
                j++;
            }
        }

        return parseHhmmssTokenToMsMain(token, timeMs);
    }

    return false;
}

bool extractFirstGpsTimeOfDayMsMain(long& timeMs)
{
    // Replay startup now reuses the first valid timestamp already found during
    // cadence derivation. If the early derive window did not contain a usable
    // GPS time token, we intentionally fall back to zero-offset alignment.
    if (!gpsReplayFirstTimeCachedMain)
        return false;

    timeMs = gpsReplayFirstTimeMsMain;
    return true;
}

bool extractFirstMagTimeOfDayMsMain(long& timeMs)
{
    // Replay startup now reuses the first valid timestamp already found during
    // cadence derivation. If the early derive window did not contain a usable
    // MAG time token, we intentionally fall back to zero-offset alignment.
    if (!magReplayFirstTimeCachedMain)
        return false;

    timeMs = magReplayFirstTimeMsMain;
    return true;
}

long computeReplayStartOffsetSourceMsMain()
{
    // Compare the first valid timestamps captured during the early derive
    // passes and keep the shortest signed separation in the +/-12 hour window.
    // This avoids a second long-file scan during startup.
    long gpsStartMs = 0;
    long magStartMs = 0;

    if (!extractFirstGpsTimeOfDayMsMain(gpsStartMs))
        return 0;

    if (!extractFirstMagTimeOfDayMsMain(magStartMs))
        return 0;

    long delta = magStartMs - gpsStartMs;

    if (delta > 43200000L)
        delta -= 86400000L;
    else if (delta < -43200000L)
        delta += 86400000L;

    return delta;
}

unsigned long deriveGpsCapturePeriodMsMain()
{
    // Derive cadence from unique GPS epoch transitions.
    // Multiple sentence types may share one epoch, so we only count
    // transitions between distinct time keys.
    //
    // IMPORTANT:
    // - Replay files can be very long.
    // - We only need a small early sample to estimate cadence.
    // - This keeps simulator startup fast.
    File replayFile = SD.open(REPLAY_GPS_FILE_PATH, FILE_READ);
    if (!replayFile)
        return 1000;

    gpsReplayFirstTimeCachedMain = false;
    gpsReplayFirstTimeMsMain = 0;

    long previousMs = -1;
    String previousKey;
    unsigned long deltaSum = 0;
    int deltaCount = 0;
    String line;
    int linesScanned = 0;

    while (deltaCount < GPS_REPLAY_DERIVE_TARGET_TRANSITIONS_MAIN &&
           linesScanned < GPS_REPLAY_DERIVE_MAX_LINES_MAIN &&
           readNextReplayLineNoWrapMain(replayFile, line))
    {
        linesScanned++;
        String key = gpsEpochKeyFromLineMain(line);

        if (key.length() == 0)
            continue;

        // Only sample one transition per GPS epoch to ignore duplicate sentence types.
        if (key == previousKey)
            continue;

        String rawTime = tokenAtMain(line, 1);
        long currentMs = 0;
        if (!parseHhmmssTokenToMsMain(rawTime, currentMs))
            continue;

        if (!gpsReplayFirstTimeCachedMain)
        {
            // Preserve the first valid GPS timestamp seen in the same early
            // scan used for cadence derivation so startup does not rescan.
            gpsReplayFirstTimeMsMain = currentMs;
            gpsReplayFirstTimeCachedMain = true;
        }

        if (previousMs >= 0)
        {
            long delta = currentMs - previousMs;
            if (delta <= 0)
                delta += 86400000L;

            if (delta >= 100 && delta <= 10000)
            {
                deltaSum += (unsigned long)delta;
                deltaCount++;
            }
        }

        previousMs = currentMs;
        previousKey = key;
    }

    replayFile.close();

    if (deltaCount == 0)
        return 1000;

    return deltaSum / (unsigned long)deltaCount;
}

unsigned long deriveMagCapturePeriodMsMain()
{
    // Derive cadence from MAG time-of-day deltas.
    // Use first valid subset and average bounded deltas for robustness.
    //
    // IMPORTANT:
    // - Replay files can be very long.
    // - We only sample a small early block of valid lines.
    // - This keeps replay startup responsive.
    File replayFile = SD.open(REPLAY_MAG_FILE_PATH, FILE_READ);
    if (!replayFile)
        return 500;

    magReplayFirstTimeCachedMain = false;
    magReplayFirstTimeMsMain = 0;

    long previousMs = -1;
    unsigned long deltaSum = 0;
    int deltaCount = 0;
    String line;
    int linesScanned = 0;

    while (deltaCount < MAG_REPLAY_DERIVE_TARGET_DELTAS_MAIN &&
           linesScanned < MAG_REPLAY_DERIVE_MAX_LINES_MAIN &&
           readNextReplayLineNoWrapMain(replayFile, line))
    {
        linesScanned++;
        long currentMs = 0;
        if (!extractTimeOfDayMsFromLineMain(line, currentMs))
            continue;

        if (!magReplayFirstTimeCachedMain)
        {
            // Preserve the first valid MAG timestamp seen in the same early
            // scan used for cadence derivation so startup does not rescan.
            magReplayFirstTimeMsMain = currentMs;
            magReplayFirstTimeCachedMain = true;
        }

        if (previousMs >= 0)
        {
            long delta = currentMs - previousMs;
            if (delta <= 0)
                delta += 86400000L;

            if (delta >= 20 && delta <= 5000)
            {
                deltaSum += (unsigned long)delta;
                deltaCount++;
            }
        }

        previousMs = currentMs;
    }

    replayFile.close();

    if (deltaCount == 0)
        return 500;

    return deltaSum / (unsigned long)deltaCount;
}

void processMagnetometerLineMain(const String& line)
{
    logger.logMagRaw(line);
    #if ENABLE_DISPLAY_STACK
    displayMagCom.addSentence(line);
    #endif

    if (!magParser.parse(line.c_str()))
        return;

    float field = magParser.getField();

    magAvg.addSample(field);
    magAverage = magAvg.getAverage();
    magDeviation = field - magAverage;
    magSamples++;

    uint16_t alarmFrequency = getAlarmFrequency(magDeviation);
    alarmActive = (alarmFrequency > 0);
    if (alarmActive)
        ledcWriteTone(BUZZER_PIN, alarmFrequency);
    else
        ledcWriteTone(BUZZER_PIN, 0);

    if (displayReady)
    {
        #if ENABLE_DISPLAY_STACK
        displayMag.update(
            field,
            magParser.getDepth(),
            magAverage,
            magDeviation
        );

        displayMagCom.updateTelemetry(
            magParser.getDate(),
            magParser.getTime(),
            magParser.getSecond(),
            gpsHandler.getDateString(),
            gpsHandler.getTimeString(),
            field,
            magParser.getDepth(),
            magParser.getSignal(),
            magParser.getLeak(),
            magParser.getMeasurementTime(),
            magParser.getQuality(),
            magParser.getFlags()
        );
        #endif
    }
}

void processGPSLineMain(const String& line)
{
    logger.logGPSRaw(line);

    if (displayReady)
    {
        #if ENABLE_DISPLAY_STACK
        displayGPSCom.addNMEALine(line);
        #endif
    }

    if (!gpsHandler.parse(line))
        return;

    float lat = gpsHandler.getLatitude();
    float lon = gpsHandler.getLongitude();

    gpsDistance.update(lat, lon);
    gpsPath.add(lat, lon, magDeviation);

    if (displayReady)
    {
        #if ENABLE_DISPLAY_STACK
        displayMap.updatePosition(
            lat,
            lon,
            gpsHandler.getSpeed(),
            gpsHandler.getHeading(),
            magDeviation
        );
        displayGPSCom.updateGPS(
            lat,
            lon,
            gpsHandler.getSpeed(),
            gpsHandler.getHeading(),
            gpsHandler.getFixQuality(),
            gpsHandler.getSatellites(),
            gpsHandler.getHDOP(),
            gpsHandler.getTimeString(),
            gpsHandler.getDateString()
        );
        #endif
    }
}

void feedGpsEpochFromReplayMain()
{
    if (!gpsReplayReadyMain)
        return;

    String epochKey;
    bool sawTimedSentence = false;
    uint8_t consumed = 0;

    while (consumed < 32)
    {
        String line;
        if (gpsReplayHasPendingLineMain)
        {
            line = gpsReplayPendingLineMain;
            gpsReplayHasPendingLineMain = false;
        }
        else if (!readNextReplayLineMain(gpsReplayFileMain, REPLAY_GPS_FILE_PATH, line))
        {
            gpsReplayReadyMain = false;
            return;
        }

        String lineKey = gpsEpochKeyFromLineMain(line);

        if (lineKey.length() > 0)
        {
            if (!sawTimedSentence)
            {
                sawTimedSentence = true;
                epochKey = lineKey;
            }
            else if (lineKey != epochKey)
            {
                gpsReplayPendingLineMain = line;
                gpsReplayHasPendingLineMain = true;
                break;
            }
        }

        processGPSLineMain(line);
        consumed++;
    }
}

void feedMagSampleFromReplayMain()
{
    if (!magReplayReadyMain)
        return;

    String line;
    if (!readNextReplayLineMain(magReplayFileMain, REPLAY_MAG_FILE_PATH, line))
    {
        magReplayReadyMain = false;
        return;
    }

    processMagnetometerLineMain(line);
}

void updateReplayDataMain()
{
    // Replay scheduler:
    // - GPS is advanced in epoch groups.
    // - MAG is advanced per sample line.
    // - Catch-up caps avoid long blocking loops if timing slips.
    unsigned long now = millis();
    unsigned long gpsPeriod = max(1UL, gpsReplayPeriodMsMain);
    unsigned long magPeriod = max(1UL, magReplayPeriodMsMain);

    uint8_t gpsCatchups = 0;
    while (now - lastGpsReplayMs >= gpsPeriod && gpsCatchups < MAX_GPS_CATCHUP_STEPS_PER_LOOP_MAIN)
    {
        lastGpsReplayMs += gpsPeriod;
        feedGpsEpochFromReplayMain();
        gpsCatchups++;
    }

    uint8_t magCatchups = 0;
    while (now - lastMagReplayMs >= magPeriod && magCatchups < MAX_MAG_CATCHUP_STEPS_PER_LOOP_MAIN)
    {
        lastMagReplayMs += magPeriod;
        feedMagSampleFromReplayMain();
        magCatchups++;
    }
}
#endif


/***********************************************************************
SERIAL DATA CAPTURE
Reads characters from UART and pushes them into FIFO buffers
***********************************************************************/

void captureSerialStreams()
{
    while (MAG_SERIAL.available())
    {
        char c = MAG_SERIAL.read();
        magFIFO.push(c);
    }

    while (GPS_SERIAL.available())
    {
        char c = GPS_SERIAL.read();
        gpsFIFO.push(c);
    }
}
/***********************************************************************
MAGNETOMETER PROCESSING PIPELINE
***********************************************************************/

void processMagnetometer()
{
    String line;

    while (magFIFO.getLine(line))
    {

        /***********************************************************
        RAW DATA LOGGING
        ***********************************************************/

        logger.logMagRaw(line);

    // Mirror every magnetometer sentence received
     #if ENABLE_DISPLAY_STACK
     displayMagCom.addSentence(line);
     #endif

        /***********************************************************
        MAGNETOMETER PARSING
        ***********************************************************/

        if (magParser.parse(line.c_str()))
        {

            // Live-mode cadence estimate for time-based moving-average sizing.
            #if !ENABLE_FILE_REPLAY_SIM
            {
                const unsigned long nowMs = millis();
                static unsigned long lastMagParsedMsMain = 0;
                if (lastMagParsedMsMain != 0)
                {
                    const unsigned long deltaMs = nowMs - lastMagParsedMsMain;
                    if (deltaMs >= 20 && deltaMs <= 5000)
                        magSourcePeriodMsMain = (magSourcePeriodMsMain * 7UL + deltaMs) / 8UL;
                }
                lastMagParsedMsMain = nowMs;
            }
            #endif

            float field = magParser.getField();


            /*******************************************************
            MOVING AVERAGE BASELINE ENGINE
            *******************************************************/

            magAvg.addSample(field);

            magAverage = magAvg.getAverage();
            magDeviation = field - magAverage;

            magSamples++;

            /*******************************************************
            AUDIBLE ALARM - Based on magnetometer deviation
            Maps deviation to frequency (0 Hz = disabled/silent)
            Frequency increases with deviation magnitude above threshold
            *******************************************************/

            uint16_t alarmFrequency = getAlarmFrequency(magDeviation);
            alarmActive = (alarmFrequency > 0);

            if (alarmActive) {
                // Alarm active: pass frequency to PWM tone on ESP32-S3
                // Higher deviation -> higher pitch as defined by MagAlarmUtils
                ledcWriteTone(BUZZER_PIN, alarmFrequency);
                // Optional debug output
                // Serial.printf("Alarm tone %u Hz (active=true)\n", alarmFrequency);
            } else {
                // Silent if below MAG_DEVIATION_LOW threshold
                ledcWriteTone(BUZZER_PIN, 0);
                // Optional debug output
                // Serial.println("Alarm silent (active=false)");
            }

            /*******************************************************
            DISPLAY UPDATE
            *******************************************************/

                if (displayReady)
                {
                    #if ENABLE_DISPLAY_STACK
                    displayMag.update(
                        field,
                        magParser.getDepth(),
                        magAverage,
                        magDeviation
                    );

                    displayMagCom.updateTelemetry(
                        magParser.getDate(),
                        magParser.getTime(),
                        magParser.getSecond(),
                        gpsHandler.getDateString(),
                        gpsHandler.getTimeString(),
                        field,
                        magParser.getDepth(),
                        magParser.getSignal(),
                        magParser.getLeak(),
                        magParser.getMeasurementTime(),
                        magParser.getQuality(),
                        magParser.getFlags()
                    );
                    #endif
                }
        }
    }
}


/***********************************************************************
GPS PROCESSING PIPELINE
***********************************************************************/

void processGPS()
{
    String line;

    while (gpsFIFO.getLine(line))
    {

        /***********************************************************
        RAW NMEA LOGGING
        ***********************************************************/

        logger.logGPSRaw(line);

       // Mirror every GPS sentence received
       if (displayReady)
       {
           #if ENABLE_DISPLAY_STACK
           displayGPSCom.addNMEALine(line);
           #endif
       }

        /***********************************************************
        GPS PARSER
        ***********************************************************/

        if (gpsHandler.parse(line))
        {

            // Live-mode cadence estimate for map-history sizing policy.
            #if !ENABLE_FILE_REPLAY_SIM
            {
                const unsigned long nowMs = millis();
                static unsigned long lastGpsParsedMsMain = 0;
                if (lastGpsParsedMsMain != 0)
                {
                    const unsigned long deltaMs = nowMs - lastGpsParsedMsMain;
                    if (deltaMs >= 100 && deltaMs <= 10000)
                        gpsSourcePeriodMsMain = (gpsSourcePeriodMsMain * 7UL + deltaMs) / 8UL;
                }
                lastGpsParsedMsMain = nowMs;
            }
            #endif

            float lat = gpsHandler.getLatitude();
            float lon = gpsHandler.getLongitude();

            gpsDistance.update(lat, lon);

            // PathFIFO requires (float lat, float lon, float deviation)
            gpsPath.add(lat, lon, magDeviation);


            /*******************************************************
            DISPLAY MAP UPDATE
            *******************************************************/

            if (displayReady)
            {
                #if ENABLE_DISPLAY_STACK
                displayMap.updatePosition(
                    lat,
                    lon,
                    gpsHandler.getSpeed(),
                    gpsHandler.getHeading(),
                    magDeviation
                );

                displayGPSCom.updateGPS(
                    lat,
                    lon,
                    gpsHandler.getSpeed(),
                    gpsHandler.getHeading(),
                    gpsHandler.getFixQuality(),
                    gpsHandler.getSatellites(),
                    gpsHandler.getHDOP(),
                    gpsHandler.getTimeString(),
                    gpsHandler.getDateString()
                );
                #endif
            }
        }
    }
}


/***********************************************************************
DISPLAY BUTTON CONTROLLER (Interrupt-driven detection, Main.cpp logic)
***********************************************************************/

void processDisplayButtons()
{
    #if ENABLE_DISPLAY_STACK
    if (!displayReady)
        return;

    int buttonIndex = consumePendingButtonIndex();

    if (buttonIndex < 0) {
        return;
    }

    unsigned long now = millis();
    if (now - buttonLastFired[buttonIndex] <= BUTTON_COOLDOWN_MS) {
        return;
    }
    buttonLastFired[buttonIndex] = now;

    Serial.print("BTN");
    Serial.println(buttonIndex + 1);

    // Button functions from Main.cpp original logic:
    // Button 0 -> Cycle displays
    // Button 1 -> Menu Down
    // Button 2 -> Menu Up  
    // Button 3 -> Menu Select
    
    if (buttonIndex == 0) {
        // Cycle through displays
        currentDisplay = (DisplayMode)((currentDisplay + 1) % 5);
    } else if (buttonIndex == 1) {
        // Menu Down
        if(currentDisplay == DISPLAY_GPSCOM)
            displayGPSCom.menuDown();
        if(currentDisplay == DISPLAY_MAGCOM)
            displayMagCom.menuDown();
        if(currentDisplay == DISPLAY_MAG)
            displayMag.menuDown();
        if(currentDisplay == DISPLAY_PARAMS)
            displayParams.menuDown();
    } else if (buttonIndex == 2) {
        // Menu Up
        if(currentDisplay == DISPLAY_GPSCOM)
            displayGPSCom.menuUp();
        if(currentDisplay == DISPLAY_MAGCOM)
            displayMagCom.menuUp();
        if(currentDisplay == DISPLAY_MAG)
            displayMag.menuUp();
        if(currentDisplay == DISPLAY_PARAMS)
            displayParams.menuUp();
    } else if (buttonIndex == 3) {
        // Menu Select
        if(currentDisplay == DISPLAY_GPSCOM)
            displayGPSCom.menuSelect();
        if(currentDisplay == DISPLAY_MAGCOM)
            displayMagCom.menuSelect();
        if(currentDisplay == DISPLAY_MAG)
            displayMag.menuSelect();
        if(currentDisplay == DISPLAY_PARAMS)
            displayParams.menuSelect();
    }
    #endif

}


/***********************************************************************
DISPLAY RENDER CONTROLLER
***********************************************************************/

void renderCurrentDisplay()
{
    #if ENABLE_DISPLAY_STACK
    if (!displayReady)
        return;

    bool forceRender = false;

    if(currentDisplay != lastDisplay)
    {
        tft.fillScreen(ILI9341_BLACK);

        switch(currentDisplay)
        {
            case DISPLAY_MAG:
                displayMag.begin();
                break;

            case DISPLAY_MAP:
                displayMap.begin();
                break;

            case DISPLAY_GPSCOM:
                displayGPSCom.begin();
                break;

            case DISPLAY_MAGCOM:
                displayMagCom.begin();
                break;

            case DISPLAY_PARAMS:
                displayParams.begin();
                break;
        }

        lastDisplay = currentDisplay;
        forceRender = true;
    }

    unsigned long renderIntervalMs = DISPLAY_RENDER_INTERVAL_MAG_MS;
    switch (currentDisplay)
    {
        case DISPLAY_MAG:
            renderIntervalMs = DISPLAY_RENDER_INTERVAL_MAG_MS;
            break;
        case DISPLAY_MAP:
            renderIntervalMs = DISPLAY_RENDER_INTERVAL_MAP_MS;
            break;
        case DISPLAY_GPSCOM:
            renderIntervalMs = DISPLAY_RENDER_INTERVAL_GPSCOM_MS;
            break;
        case DISPLAY_MAGCOM:
            renderIntervalMs = DISPLAY_RENDER_INTERVAL_MAGCOM_MS;
            break;
        case DISPLAY_PARAMS:
            renderIntervalMs = DISPLAY_RENDER_INTERVAL_PARAMS_MS;
            break;
    }

    unsigned long now = millis();
    if (!forceRender && (now - lastDisplayRenderMs) < renderIntervalMs)
        return;

    lastDisplayRenderMs = now;

    if (forceRender)
    {
        // Ensure first frame after a mode switch always renders immediately.
        lastDisplayRenderMs = now;
    }

    switch(currentDisplay)
    {
        case DISPLAY_MAG:
            displayMag.render();
            break;

        case DISPLAY_MAP:
            displayMap.render();
            break;

        case DISPLAY_GPSCOM:
            displayGPSCom.render();
            break;

        case DISPLAY_MAGCOM:
            displayMagCom.render();
            break;

        case DISPLAY_PARAMS:
            displayParams.render();
            break;
    }
    #endif
}


/***********************************************************************
OPERATOR COMMAND INTERFACE
***********************************************************************/

void processUserCommands()
{
    if (!Serial.available())
        return;

    String cmd = Serial.readStringUntil('\n');
    cmd.trim();

    if (cmd == "start")
    {
        String dateString = getDateString();
        logger.startRecording(dateString);
#if ENABLE_FILE_REPLAY_SIM
        replaySimRecordingMain = true;
#endif

        Serial.println("Recording Started");
    }

    if (cmd == "stop")
    {
        logger.stopRecording();
#if ENABLE_FILE_REPLAY_SIM
        replaySimRecordingMain = false;
#endif

        Serial.println("Recording Stopped");
    }

    if (cmd == "restart")
    {
        String dateString = getDateString();
        logger.restartRecording(dateString);
#if ENABLE_FILE_REPLAY_SIM
        replaySimRecordingMain = true;
#endif

        Serial.println("Recording Restarted");
    }
}
/***********************************************************************
CHECK IF MOVING AVERAGE WINDOW CHANGED FROM MENU
***********************************************************************/
void checkMovingAverageResize()
{
    #if ENABLE_DISPLAY_STACK
    const unsigned long nowMs = millis();
    if (nowMs - lastMovingAverageRecalcMsMain < MOVING_AVG_RECALC_INTERVAL_MS_MAIN)
        return;

    lastMovingAverageRecalcMsMain = nowMs;

    const int requestedSizeFromD4 = displayMagCom.getMovingAverageSize();
    const int requestedSizeFromD5 = displayParams.getMovingAverageSize();

    int requestedSize = gMovingAverageMenuSizeMain;

    // Display5 is the primary parameter screen, but D4 remains a valid
    // duplicate control path for backward compatibility.
    if (requestedSizeFromD5 != gMovingAverageMenuSizeMain)
        requestedSize = requestedSizeFromD5;
    else if (requestedSizeFromD4 != gMovingAverageMenuSizeMain)
        requestedSize = requestedSizeFromD4;

    requestedSize = (requestedSize <= 4) ? 4 : 480;

    if (requestedSize != gMovingAverageMenuSizeMain)
    {
        gMovingAverageMenuSizeMain = requestedSize;
        displayMagCom.setMovingAverageSize(gMovingAverageMenuSizeMain);
        displayParams.setMovingAverageSize(gMovingAverageMenuSizeMain);
    }

    const int avgDurationSeconds = movingAverageDurationSecondsFromMenuMain(requestedSize);
    const int desiredSamples = computeMovingAverageSamplesMain(avgDurationSeconds, magSourcePeriodMsMain);

    if(desiredSamples != currentAverageWindow)
    {
        currentAverageWindow = desiredSamples;

        magAvg.init(currentAverageWindow);

        Serial.print("Moving average window changed to ");
        Serial.println(currentAverageWindow);
        Serial.print("CHK: avg duration s=");
        Serial.print(avgDurationSeconds);
        Serial.print(" sourcePeriodMs=");
        Serial.println(magSourcePeriodMsMain);
    }
    #endif
}

/***********************************************************************
MAIN LOOP
***********************************************************************/

void loop()
{
    if (!systemInitialized)
        return;

#if ENABLE_FILE_REPLAY_SIM
    updateReplayDataMain();
#else
    captureSerialStreams();

    processMagnetometer();

    processGPS();
#endif

    checkMovingAverageResize();

    /***************************************************************
    UPDATE COMPASS
    ***************************************************************/

   if (millis() - compassTimer >= COMPASS_INTERVAL)
   {
    compassTimer = millis();

    #if ENABLE_FILE_REPLAY_SIM
    // Match probe behavior in simulation: compass follows GPS course with
    // small drift and bounded slew, so heading-up map visibly rotates.
    float course = gpsHandler.getHeading();
    if (!isfinite(course) || course < 0.0f || course >= 360.0f)
        course = compassHeading;

    const float drift = 3.0f * sinf((float)millis() * 0.0013f);
    const float targetHeading = wrap360Main(course + drift);
    const float delta = shortestHeadingDeltaMain(compassHeading, targetHeading);
    const float step = clampfMain(delta, -5.0f, 5.0f);

    compassHeading = wrap360Main(compassHeading + step);
    compassCalibrationRequired = (((int)(millis() / 10000UL)) % 2) == 1;
    #else
    compass.update();
    compassHeading = compass.getHeading();
    compassCalibrationRequired = compass.calibrationNeeded();
    #endif

    #if ENABLE_DISPLAY_STACK
    if (displayReady)
        displayGPSCom.updateCompass(compassHeading, compassCalibrationRequired);
    #endif
   }

    processUserCommands();

    processDisplayButtons();

    renderCurrentDisplay();

/***************************************************************
WIFI SERVER HANDLER
Runs for 5 minutes after boot then turns off
***************************************************************/

if (wifiActive)
{
    server.handleClient();

    if (millis() - wifiStartTime > 300000) // 5 minutes
    {
        WiFi.softAPdisconnect(true);
        wifiActive = false;

        Serial.println("WiFi Access Point Stopped");
    }
}
}