#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define I2C_SDA 8
#define I2C_SCL 9
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

const int PIN_H_REP = 1;     // 电位器1: ToF高度
const int PIN_E_RADAR = 2;   // 电位器2: 雷达微动能量
const int PIN_DOOR = 4;      // 滑动开关: 门磁
const int PIN_CANCEL = 5;    // 按钮: 取消 (另一端接GND)
const int PIN_SOS = 6;       // 按钮: 紧急 (另一端接GND)

enum SysState { STATE_NORMAL, STATE_FALLING, STATE_CHECKING, STATE_PRE_ALARM, STATE_ALARMED };
SysState state = STATE_NORMAL;

int h_val = 200, radar_val = 0, last_h = 200;
int v_speed = 0; 
int count = 10;
unsigned long t_mark = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(PIN_DOOR, INPUT);
  pinMode(PIN_CANCEL, INPUT_PULLUP); // 关键：PULLUP 意味着未按下时是高电平
  pinMode(PIN_SOS, INPUT_PULLUP);

  // 强制指定 ESP32-S3 的 I2C 引脚
  Wire.begin(I2C_SDA, I2C_SCL);

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED Check Failed!");
    while(1);
  }
  
  display.clearDisplay();
  display.display();
}

void loop() {
  // 1. 数据采集与映射
  int rawH = analogRead(PIN_H_REP);
  // 反向映射：0(左)=250cm(站立), 4095(右)=0cm(倒地)
  // 如果你发现方向反了，把下面改成 map(rawH, 0, 4095, 0, 250);
  h_val = map(rawH, 0, 4095, 250, 0); 
  
  int rawR = analogRead(PIN_E_RADAR);
  radar_val = map(rawR, 0, 4095, 0, 100);

  // 2. 状态机逻辑
  v_speed = last_h - h_val; // 计算下降速度
  last_h = h_val;

  // SOS 紧急中断
  if (digitalRead(PIN_SOS) == LOW) {
    state = STATE_ALARMED;
  }

  switch(state) {
    case STATE_NORMAL:
      // 【修改点1】降低速度阈值：从35降为15，更容易触发
      if (v_speed > 15 && h_val < 180) { // 检测到下坠趋势
        state = STATE_FALLING;
        t_mark = millis();
        Serial.println(">> FSM: Entered FALLING state");
      }
      break;

    case STATE_FALLING:
      // 【修改点2】落地高度放宽到 60cm
      if (h_val < 60) { 
        state = STATE_CHECKING;
        t_mark = millis();
        Serial.println(">> FSM: Entered CHECKING state");
      } 
      // 【修改点3】延长判定时间窗：从800ms改为2000ms (给你2秒钟拧到底)
      else if (millis() - t_mark > 2000) { 
        state = STATE_NORMAL;
        Serial.println(">> FSM: Timeout (Not a fall), back to NORMAL");
      }
      break;

    case STATE_CHECKING:
      // 检查到呼吸信号（活体）
      if (radar_val > 25) { 
        if (millis() - t_mark > 1500) {
          state = STATE_PRE_ALARM;
          count = 10;
          t_mark = millis();
          Serial.println(">> FSM: Vitals confirmed. PRE-ALARM!");
        }
      } else {
        // 没呼吸判定为物体掉落
        if (millis() - t_mark > 3000) {
            state = STATE_NORMAL; 
            Serial.println(">> FSM: No vitals (Fake Fall). Back to NORMAL");
        }
      }
      break;

    case STATE_PRE_ALARM:
      if (millis() - t_mark >= 1000) {
        count--;
        t_mark = millis();
      }
      if (digitalRead(PIN_CANCEL) == LOW) state = STATE_NORMAL; // 手动取消
      if (count <= 0) {
        state = STATE_ALARMED;
        Serial.println(">>> MQTT ALERT SENT!");
      }
      break;

    case STATE_ALARMED:
      if (digitalRead(PIN_CANCEL) == LOW) state = STATE_NORMAL;
      break;
  }

  draw();
  delay(100);
}

void draw() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  // 绘制门禁和雷达状态
  display.setCursor(0,0);
  display.print(digitalRead(PIN_DOOR) ? "DOOR:OPEN" : "DOOR:CLOSED");
  display.setCursor(80,0);
  display.print("RAD:"); display.print(radar_val);

  // 绘制高度信息
  display.setCursor(0, 15);
  display.print("H: "); display.print(h_val); display.print("cm ");
  
  // 【修改点4】显示实时速度，方便调试手感
  display.setCursor(70, 15);
  display.print("V:"); display.print(v_speed);

  // 绘制核心状态
  display.setTextSize(2);
  display.setCursor(0, 35);
  switch(state) {
    case STATE_NORMAL:    display.print("SYS: OK"); break;
    case STATE_FALLING:   display.print("FALLING!"); break;
    case STATE_CHECKING:  display.print("CHK VITALS"); break;
    case STATE_PRE_ALARM: 
      display.print("WARN: "); display.print(count); display.print("s"); 
      break;
    case STATE_ALARMED:   
      display.fillRect(0, 32, 128, 32, WHITE);
      display.setTextColor(BLACK);
      display.print("!!ALARM!!"); 
      break;
  }
  display.display();
}