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

// 引脚定义
#define BUZZER_PIN 23
#define LED_PIN 2
#define BUTTON_PIN 5

// OLED参数
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define OLED_ADDR 0x3C

// 传感器对象
Adafruit_MPU6050 mpu;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// 检测参数
const float FALL_THRESHOLD = 2.5;    // 加速度阈值 (g-force)
const float ANGLE_THRESHOLD = 45.0;  // 角度阈值 (度)
const unsigned long ALARM_DURATION = 10000;  // 报警持续时间(ms)

// 状态变量
bool fallDetected = false;
unsigned long fallTime = 0;
bool buttonPressed = false;
unsigned long lastDisplayUpdate = 0;

void setup() {
  Serial.begin(115200);
  
  // 初始化硬件引脚
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  digitalWrite(BUZZER_PIN, LOW);
  digitalWrite(LED_PIN, HIGH);  // 正常状态指示灯亮
  
  // 初始化OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println(F("SSD1306分配失败"));
    for(;;); // 停止执行
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("系统启动中...");
  display.display();
  delay(1000);
  
  // 初始化MPU6050
  if (!mpu.begin()) {
    Serial.println("无法找到MPU6050芯片!");
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("MPU6050初始化失败!");
    display.display();
    while (1) {
      // 错误闪烁
      digitalWrite(LED_PIN, !digitalRead(LED_PIN));
      delay(200);
    }
  }
  
  // 配置MPU6050参数
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  
  Serial.println("跌倒检测器已启动");
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("跌倒检测器已启动");
  display.println("状态: 正常");
  display.display();
  delay(1000);
}

void updateDisplay(float accel, float pitch, float roll, bool alert = false) {
  display.clearDisplay();
  
  // 显示标题
  display.setTextSize(1);
  display.setCursor(0,0);
  display.println("跌倒检测系统");
  display.drawLine(0, 10, 128, 10, SSD1306_WHITE);
  
  // 显示传感器数据
  display.setCursor(0,15);
  display.print("加速度: ");
  display.print(accel);
  display.println(" g");
  
  display.setCursor(0,25);
  display.print("俯仰角: ");
  display.print(pitch);
  display.println("°");
  
  display.setCursor(0,35);
  display.print("滚转角: ");
  display.print(roll);
  display.println("°");
  
  // 显示状态
  display.setCursor(0,50);
  display.setTextSize(1);
  if (alert) {
    display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // 反色显示
    display.println("  ALERT: FALL DETECTED!  ");
    display.setTextColor(SSD1306_WHITE);
  } else {
    display.print("状态: ");
    display.println("正常");
  }
  
  // 添加电池图标
  display.drawRect(100, 0, 20, 10, SSD1306_WHITE);
  display.fillRect(120, 3, 2, 4, SSD1306_WHITE);
  display.fillRect(102, 2, 16, 6, SSD1306_WHITE); // 全电量
  
  // 添加状态指示器
  if(fallDetected) {
    // 闪烁的警报指示器
    if(millis() % 500 < 250) {
      display.fillCircle(115, 45, 5, SSD1306_WHITE);
    }
  } else {
    // 绿色状态指示器
    display.drawCircle(115, 45, 5, SSD1306_WHITE);
  }
  
  display.display();
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // 计算合加速度 (g-force)
  float acceleration = sqrt(sq(a.acceleration.x) + 
                      sq(a.acceleration.y) + 
                      sq(a.acceleration.z)) / 9.8;
  
  // 计算俯仰角和滚转角 (度)
  float pitch = atan2(-a.acceleration.x, sqrt(sq(a.acceleration.y) + sq(a.acceleration.z))) * 180 / PI;
  float roll = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;

  // 检测跌倒条件
  if (!fallDetected && 
      acceleration > FALL_THRESHOLD && 
      (abs(pitch) > ANGLE_THRESHOLD || abs(roll) > ANGLE_THRESHOLD)) {
    
    fallDetected = true;
    fallTime = millis();
    
    Serial.println("跌倒检测!");
    Serial.print("加速度: "); Serial.print(acceleration); Serial.println(" g");
    Serial.print("俯仰角: "); Serial.print(pitch); Serial.println("°");
    Serial.print("滚转角: "); Serial.print(roll); Serial.println("°");
    
    // 触发报警
    digitalWrite(BUZZER_PIN, HIGH);
    digitalWrite(LED_PIN, LOW);  // 报警状态指示灯灭
  }

  // 检查取消按钮
  if (digitalRead(BUTTON_PIN) == LOW) {
    if (!buttonPressed) {
      buttonPressed = true;
      if (fallDetected) {
        fallDetected = false;
        digitalWrite(BUZZER_PIN, LOW);
        digitalWrite(LED_PIN, HIGH);
        Serial.println("报警手动取消");
      }
    }
  } else {
    buttonPressed = false;
  }

  // 处理报警状态
  if (fallDetected) {
    // 闪烁LED
    digitalWrite(LED_PIN, millis() % 200 < 100);
    
    // 检查报警持续时间
    if (millis() - fallTime > ALARM_DURATION) {
      fallDetected = false;
      digitalWrite(BUZZER_PIN, LOW);
      digitalWrite(LED_PIN, HIGH);  // 恢复正常状态
      Serial.println("报警自动结束");
    }
  }

  // 更新OLED显示 (每秒5次)
  if (millis() - lastDisplayUpdate > 200) {
    updateDisplay(acceleration, pitch, roll, fallDetected);
    lastDisplayUpdate = millis();
  }

  delay(50);  // 采样间隔
}