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

#define PAPI_PIN          6   

// 📡 測距儀腳位配置
#define SENSOR_A_TRIG    10   // 近 (基準 100)
#define SENSOR_A_ECHO    11   
#define SENSOR_B_TRIG    12   // 中 (基準 175)
#define SENSOR_B_ECHO    13   
#define SENSOR_C_TRIG    A5   // 遠 (基準 250)
#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);

float currentY = 64.0; 
unsigned long lastExecutionTime = 0; 

float activeHeight = 250.0; 
int activeMode = 0; 

// 初始有效歷史值
float lastValidA = 100.0;
float lastValidB = 175.0;
float lastValidC = 250.0;

// 儲存前一次的變化量（雙重差分過濾）
float prevDiffA = 0.0;
float prevDiffB = 0.0;
float prevDiffC = 0.0;

char currentWarning[25] = "INIT"; 

// 💡 實體燈條控制
void setPapi(byte l0, byte l1, byte l2, byte l3) {
  papiStrip.setPixelColor(0, l0 ? papiStrip.Color(255,255,255) : papiStrip.Color(255,0,0));
  papiStrip.setPixelColor(1, l1 ? papiStrip.Color(255,255,255) : papiStrip.Color(255,0,0));
  papiStrip.setPixelColor(2, l2 ? papiStrip.Color(255,255,255) : papiStrip.Color(255,0,0));
  papiStrip.setPixelColor(3, l3 ? papiStrip.Color(255,255,255) : papiStrip.Color(255,0,0));
}

// 📡 超音波讀取
float readUltrasonic(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 20000); 
  return (duration == 0) ? -99.0 : (duration * 0.034) / 2;
}

// 🖥️ 警告螢幕更新（防止閃爍優化）
void updateWarningText(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 setup() {
  papiStrip.begin(); setPapi(1, 1, 0, 0); papiStrip.show(); 
  
  pinMode(SENSOR_A_TRIG, OUTPUT); pinMode(SENSOR_A_ECHO, INPUT);
  pinMode(SENSOR_B_TRIG, OUTPUT); pinMode(SENSOR_B_ECHO, INPUT);
  pinMode(SENSOR_C_TRIG, OUTPUT); pinMode(SENSOR_C_ECHO, INPUT);
  
  ilsDisplay.begin(); warningDisplay.begin(); warningDisplay.clear();
  strcpy(currentWarning, "");
}

void loop() {
  if (millis() - lastExecutionTime >= 30) { 
    lastExecutionTime = millis();

    // 1. 讀取感測器原始數據
    float rawA = readUltrasonic(SENSOR_A_TRIG, SENSOR_A_ECHO);
    float rawB = readUltrasonic(SENSOR_B_TRIG, SENSOR_B_ECHO);
    float rawC = readUltrasonic(SENSOR_C_TRIG, SENSOR_C_ECHO);

    // 🌟 突波過濾機制
    if (rawA >= 300.0 || rawA < 0.1) rawA = lastValidA;
    if (rawB >= 300.0 || rawB < 0.1) rawB = lastValidB;
    if (rawC >= 300.0 || rawC < 0.1) rawC = lastValidC;

    float diffA = abs(rawA - lastValidA);
    float diffB = abs(rawB - lastValidB);
    float diffC = abs(rawC - lastValidC);

    // 2. 雙重連續跳變競爭邏輯
    if (diffA > 3.0 && prevDiffA > 1.0) {
      activeMode = 2; // A 接管
      activeHeight = rawA;
    } 
    else if (diffB > 3.0 && prevDiffB > 1.0) {
      activeMode = 1; // B 接管
      activeHeight = rawB;
    } 
    else if (diffC > 3.0 && prevDiffC > 1.0) {
      activeMode = 0; // C 接管
      activeHeight = rawC;
    }
    else {
      if (activeMode == 2) activeHeight = rawA;
      if (activeMode == 1) activeHeight = rawB;
      if (activeMode == 0) activeHeight = rawC;
    }
    
    // 儲存本次歷史紀錄
    prevDiffA = diffA; prevDiffB = diffB; prevDiffC = diffC;
    lastValidA = rawA; lastValidB = rawB; lastValidC = rawC;

    float h = activeHeight;
    int targetY = 64; 

    // =================================================================
    // 📊 區間映射縮放與燈號邏輯
    // =================================================================
    
    // 🟢 C 模式 (基準 250)
    if (activeMode == 0) {
      if (h >= 260.0)      { updateWarningText("GLIDE", "ABOVE"); setPapi(1, 1, 1, 1); } 
      else if (h < 90.0)   { updateWarningText("TERRAIN", "PULL UP!"); setPapi(0, 0, 0, 0); } 
      else if (h < 240.0)  { updateWarningText("TERRAIN", "PULL UP!"); setPapi(1, 0, 0, 0); } 
      else                 { updateWarningText("", ""); setPapi(1, 1, 0, 0); } 

      if (h >= 250.0) targetY = 64 - (int)(((h > 290.0 ? 290.0 : h) - 250.0) * (50.0 / 40.0) + 0.5);
      else targetY = 64 + (int)((250.0 - (h < 0.0 ? 0.0 : h)) * (50.0 / 250.0) + 0.5);
    } 
    // 🔵 B 模式 (基準 175)
    else if (activeMode == 1) {
      if (h >= 240.0)                 { updateWarningText("GLIDE", "ABOVE"); setPapi(1, 1, 1, 1); }
      else if (h >= 185.0 && h < 240.0) { updateWarningText("GLIDE", "ABOVE"); setPapi(1, 1, 1, 0); }
      else if (h >= 165.0 && h < 185.0) { updateWarningText("", ""); setPapi(1, 1, 0, 0); }
      else if (h >= 110.0 && h < 165.0) { updateWarningText("GLIDE", "BELOW"); setPapi(1, 0, 0, 0); }
      else                            { updateWarningText("TERRAIN", "PULL UP!"); setPapi(0, 0, 0, 0); } 

      if (h >= 175.0) targetY = 64 - (int)(((h > 290.0 ? 290.0 : h) - 175.0) * (50.0 / 115.0) + 0.5);
      else targetY = 64 + (int)((175.0 - (h < 0.0 ? 0.0 : h)) * (50.0 / 175.0) + 0.5);
    } 
    // 🟡 A 模式 (基準 100)
    else if (activeMode == 2) {
      if (h > 135.0)                 { updateWarningText("GLIDE", "ABOVE"); setPapi(1, 1, 1, 1); }
      else if (h >= 110.0 && h <= 135.0) { updateWarningText("GLIDE", "ABOVE"); setPapi(1, 1, 1, 0); }
      else if (h >= 65.0 && h <= 90.0) { updateWarningText("TERRAIN", "PULL UP!"); setPapi(1, 0, 0, 0); }
      else if (h < 65.0)             { updateWarningText("TERRAIN", "PULL UP!"); setPapi(0, 0, 0, 0); } 
      else                           { updateWarningText("", ""); setPapi(1, 1, 0, 0); } 

      if (h >= 100.0) targetY = 64 - (int)(((h > 290.0 ? 290.0 : h) - 100.0) * (50.0 / 190.0) + 0.5);
      else targetY = 64 + (int)((100.0 - (h < 0.0 ? 0.0 : h)) * (50.0 / 100.0) + 0.5);
    }

    // 速度優化緩動
    currentY = currentY + ((float)targetY - currentY) * 0.40;

    // 更新實體 NeoPixel 燈條
    papiStrip.show(); 

    // 🖥️ 繪製內部主顯示器 (ILS 交叉線導引)
    ilsDisplay.firstPage(); 
    do {
      ilsDisplay.setDrawColor(1);
      ilsDisplay.drawBox(61, 0, 7, 128); 
      
      int drawY = (int)(currentY + 0.5); 
      for (int i = -3; i <= 3; i++) {
        ilsDisplay.drawLine(0, drawY + i, 127, drawY + i); 
      }
    } while ( ilsDisplay.nextPage() );
  }
}