#include <Adafruit_NeoPixel.h>
#include <Wire.h>
#include <U8g2lib.h>

#define PAPI_PIN          6   
#define SENSOR_C_TRIG    A5   
#define SENSOR_C_ECHO    A4   

U8G2_SH1107_SEEED_128X128_1_SW_I2C ilsDisplay(U8G2_R0, 3, 2, U8X8_PIN_NONE);
U8X8_SSD1306_128X64_NONAME_SW_I2C warningDisplay(4, 5, U8X8_PIN_NONE);
Adafruit_NeoPixel papiStrip = Adafruit_NeoPixel(4, PAPI_PIN, NEO_GRB + NEO_KHZ800);

int currentY = 64; // 直接記錄當前渲染的 Y 座標
unsigned long lastSonarTime = 0;
float cachedHC = 300.0; 
char currentWarning[25] = "INIT"; 

void setPapi(byte w0, byte w1, byte w2, byte r3) {
  papiStrip.setPixelColor(0, w0 ? papiStrip.Color(255,255,255) : papiStrip.Color(255,0,0));
  papiStrip.setPixelColor(1, w1 ? papiStrip.Color(255,255,255) : papiStrip.Color(255,0,0));
  papiStrip.setPixelColor(2, w2 ? papiStrip.Color(255,255,255) : papiStrip.Color(255,0,0));
  papiStrip.setPixelColor(3, r3 ? papiStrip.Color(255,0,0) : papiStrip.Color(255,255,255));
  papiStrip.show();
}

float readUltrasonic(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 25000); 
  return (duration == 0) ? -99.0 : (duration * 0.034) / 2;
}

void setup() {
  papiStrip.begin(); 
  setPapi(1, 1, 0, 1); 
  pinMode(SENSOR_C_TRIG, OUTPUT); pinMode(SENSOR_C_ECHO, INPUT);
  ilsDisplay.begin(); warningDisplay.begin(); warningDisplay.clear();
  strcpy(currentWarning, "");
}

void showBigWarning(const char* line1, const char* line2) {
  char combined[50]; snprintf(combined, sizeof(combined), "%s_%s", line1, line2);
  if (strcmp(currentWarning, combined) != 0) {
    strcpy(currentWarning, combined); warningDisplay.clear();
    warningDisplay.setFont(u8x8_font_amstrad_cpc_extended_f);
    if (strlen(line1) > 0) warningDisplay.draw2x2String(0, 0, line1);
    if (strlen(line2) > 0) warningDisplay.draw2x2String(0, 4, line2);
  }
}

void loop() {
  if (millis() - lastSonarTime >= 15) {
    lastSonarTime = millis();
    float rawCm = readUltrasonic(SENSOR_C_TRIG, SENSOR_C_ECHO);
    if (rawCm == -99.0) cachedHC = 400.0; 
    else if (rawCm <= 0.1 || rawCm > 450.0) cachedHC = 300.0;
    else cachedHC = rawCm;
  }

  if (cachedHC > 310.0) {
    showBigWarning("GLIDE", "SLOPE!"); 
    setPapi(1, 1, 1, 1); 
  } else if (cachedHC < 290.0) {
    showBigWarning("TERRAIN", "PULL UP!"); 
    setPapi(1, 0, 0, 1); 
  } else {
    showBigWarning("", ""); 
    setPapi(1, 1, 0, 1); 
  }

  // ✈️ 1:1 靜態映射（直接鎖定目標像素位置，沒有任何遲滯動畫）
  if (cachedHC >= 200.0 && cachedHC <= 400.0) {
    currentY = (int)(14.0 + (cachedHC - 200.0) * 0.5 + 0.5); 
  } else if (cachedHC < 200.0) {
    currentY = 14;
  } else if (cachedHC > 400.0) {
    currentY = 114;
  }

  ilsDisplay.firstPage();
  do {
    ilsDisplay.setDrawColor(1);
    
    // 鉛直線 (固定中央 X=64附近)
    ilsDisplay.drawBox(61, 0, 7, 128);
    
    // 水平線 (7px 寬，瞬移到位)
    for (int i = -3; i <= 3; i++) {
      ilsDisplay.drawLine(0, currentY + i, 127, currentY + i);
    }
  } while ( ilsDisplay.nextPage() );
}