/* TrailNAV OLED: milestone 1 - live GPS, one persistent waypoint and distance.
 * Generated with Codex assistance; hardware and outdoor tests still required.
 * Bearing is relative to TRUE NORTH. No compass-relative arrow is implemented.
 * Power the first bench test through USB; solar/battery circuitry is separate.
 */
#include <Wire.h>
#include <EEPROM.h>
#include <Adafruit_GFX.h>
#include <TinyGPSPlus.h>
#include "Config.h"
#include "Navigation.h"
#if TRAILNAV_DISPLAY_SH1106
#include <Adafruit_SH110X.h>
#else
#include <Adafruit_SSD1306.h>
#endif

TinyGPSPlus gps;
#if TRAILNAV_DISPLAY_SH1106
Adafruit_SH1106G display(128,64,&Wire,-1);
#else
Adafruit_SSD1306 display(128,64,&Wire,-1);
#endif
trailnav::WaypointRecord waypoint{};
bool hasWaypoint = false;
bool oledReady = false;
bool buttonRaw = false, buttonStable = false;
bool buttonArmed = false;
uint32_t rawChangedAt = 0, pressedAt = 0, lastFrameAt = 0;
const char* notice = "";
uint32_t noticeAt = 0;

bool usableFix() {
  return gps.location.isValid() && trailnav::fresh(gps.location.age(),FIX_MAX_AGE_MS) &&
         gps.satellites.isValid() && trailnav::fresh(gps.satellites.age(),QUALITY_MAX_AGE_MS) &&
         gps.satellites.value() >= MIN_SATELLITES &&
         gps.hdop.isValid() && trailnav::fresh(gps.hdop.age(),QUALITY_MAX_AGE_MS) &&
         gps.hdop.value() <= MAX_HDOP_HUNDREDTHS &&
         trailnav::coordinatesValid(gps.location.lat(),gps.location.lng());
}

void message(const char* text) {
  notice=text;
  noticeAt=millis();
  Serial.println(text);
}

void saveCurrentWaypoint() {
  if (!usableFix()) { message("Need fresh GPS fix"); return; }
  const auto candidate = trailnav::makeWaypoint(gps.location.lat(),gps.location.lng());
  if (!trailnav::waypointValid(candidate)) { message("Invalid position"); return; }
  EEPROM.put(0,candidate);
  if (!EEPROM.commit()) { message("Save failed"); return; }
  waypoint=candidate;
  hasWaypoint=true;
  message("Waypoint saved");
}

void readButton(uint32_t now) {
  const bool down=digitalRead(SAVE_BUTTON_PIN)==LOW;
  if (down!=buttonRaw) { buttonRaw=down; rawChangedAt=now; }
  if (down!=buttonStable && now-rawChangedAt>=BUTTON_DEBOUNCE_MS) {
    buttonStable=down;
    if (down) pressedAt=now;
    else if (!buttonArmed) buttonArmed=true;
    else if (!hasWaypoint || now-pressedAt>=REPLACE_HOLD_MS) saveCurrentWaypoint();
    else message("Hold 2s to replace");
  }
}

void drawFrame(uint32_t now) {
  if (!oledReady) return;
  const bool fix=usableFix();
  display.clearDisplay();
  display.setTextColor(1); // White for either monochrome driver.
  display.setTextSize(1);
  display.setCursor(0,0);
  display.print("TrailNAV ");
  display.print(fix ? "FIX " : "WAIT ");
  display.print(gps.satellites.isValid() ? gps.satellites.value() : 0);
  if (!fix) {
    display.setCursor(0,18); display.println("Waiting for GPS");
    display.println("Use outdoors");
    display.println(hasWaypoint ? "Waypoint retained" : "No waypoint saved");
  } else if (!hasWaypoint) {
    display.setCursor(0,16); display.print("LAT "); display.println(gps.location.lat(),5);
    display.print("LON "); display.println(gps.location.lng(),5);
    display.println("Press SAVE at start");
  } else {
    const double lat=gps.location.lat(),lon=gps.location.lng();
    const double targetLat=waypoint.latE7/1e7,targetLon=waypoint.lonE7/1e7;
    const double distance=trailnav::distanceM(lat,lon,targetLat,targetLon);
    display.setCursor(0,15); display.setTextSize(2);
    if (distance<1000) { display.print(distance,0); display.print(" m"); }
    else { display.print(distance/1000,2); display.print(" km"); }
    display.setTextSize(1); display.setCursor(0,36);
    if (distance<=NEAR_WAYPOINT_M) display.println("Near waypoint");
    else {
      display.print("BRG ");
      display.print(trailnav::bearingTrueDeg(lat,lon,targetLat,targetLon),0);
      display.println(" deg T");
    }
    display.println("T = true north");
  }
  display.setTextSize(1); display.setCursor(0,56);
  if (notice[0] && now-noticeAt<2500) display.print(notice);
  else if (buttonStable && hasWaypoint)
    display.print(now-pressedAt>=REPLACE_HOLD_MS ? "Release to replace" : "Hold 2s to replace");
  else display.print(hasWaypoint ? "Hold2s: new waypoint" : "Press: save waypoint");
  display.display();
}

void setup() {
  Serial.begin(115200); // Do not wait for a computer; USB-free operation is required.
  pinMode(SAVE_BUTTON_PIN,INPUT_PULLUP);
  buttonRaw=buttonStable=digitalRead(SAVE_BUTTON_PIN)==LOW;
  buttonArmed=!buttonStable;
  // A button held at boot must be released before it can trigger a save.
  pressedAt=rawChangedAt=millis();
  EEPROM.begin(256);
  EEPROM.get(0,waypoint);
  hasWaypoint=trailnav::waypointValid(waypoint);
  Wire.setSDA(I2C_SDA_PIN); Wire.setSCL(I2C_SCL_PIN); Wire.begin();
  Wire.setClock(400000);
  Wire.beginTransmission(OLED_ADDRESS);
  const bool oledAcknowledged=Wire.endTransmission()==0;
#if TRAILNAV_DISPLAY_SH1106
  oledReady=oledAcknowledged && display.begin(OLED_ADDRESS,true);
#else
  oledReady=oledAcknowledged && display.begin(SSD1306_SWITCHCAPVCC,OLED_ADDRESS,true,false);
#endif
  if (!oledReady) Serial.println("OLED missing: check type, address, wiring and supply");
  Serial1.setRX(GPS_RX_PIN); Serial1.setTX(GPS_TX_PIN);
  Serial1.setFIFOSize(256); Serial1.begin(GPS_BAUD);
  message(hasWaypoint ? "Waypoint loaded" : "No waypoint saved");
}

void loop() {
  while (Serial1.available()) gps.encode(Serial1.read());
  const uint32_t now=millis();
  readButton(now);
  if (now-lastFrameAt>=250) { lastFrameAt=now; drawFrame(now); }
}
