#include <Wire.h>
#include <MPU6050.h>

// 硬件参数配置
#define TRIG_PIN 5         // 超声波Trig引脚
#define ECHO_PIN 18        // 超声波Echo引脚
#define LED_PIN 23         // 报警LED
#define MAX_DISTANCE 400    // 超声波最大有效距离(cm)
#define INSTALL_HEIGHT 250  // 设备安装高度(cm) 需现场测量

MPU6050 mpu;

// 全局变量
float baseline_z = 1.0;    // Z轴基准加速度（重力基准）
bool isCalibrated = false; // 校准完成标志

void setup() {
  Serial.begin(115200);
  
  // 初始化硬件
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  // 初始化MPU6050
  Wire.begin();
  mpu.initialize();
  delay(500); // 延长初始化等待时间

  // 传感器校准
  calibrateSensors();
}

void loop() {
  /*----- 1. 读取加速度数据 -----*/
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);
  
  // 转换为Z轴加速度（±2g量程）
  float acc_z = constrain(az / 16384.0, -2.0, 2.0); 

  /*----- 2. 超声波测距 -----*/
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(4); // 更稳定的触发信号
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // 超时30ms=5m
  float distance_cm = duration * 0.034 / 2;
  distance_cm = constrain(distance_cm, 2, MAX_DISTANCE); // 限制有效范围

  /*----- 3. 动态基准校准 -----*/
  if (!isCalibrated) {
    baseline_z = 0.95*baseline_z + 0.05*acc_z; // 初始校准
    if (millis() > 5000) isCalibrated = true;  // 5秒校准时间
  }

  /*----- 4. 跌倒检测逻辑 -----*/
  if (isCalibrated) {
    // 条件1：Z轴加速度突变（向下）
    bool acc_trigger = (baseline_z - acc_z) > 0.8; // 变化量>0.8g
    
    // 条件2：高度下降超过安装高度的50%
    bool height_trigger = (INSTALL_HEIGHT - distance_cm) > (INSTALL_HEIGHT*0.5);
    
    // 组合判断
    if (acc_trigger && height_trigger) {
      triggerAlarm();
    }
  }

  /*----- 5. 调试输出 -----*/
  Serial.printf("Z:%.2fg | 基准:%.2fg | 距离:%.1fcm | 状态:%s\n",
                acc_z, baseline_z, distance_cm,
                (isCalibrated ? "监控中" : "校准中"));

  delay(150); // 采样间隔150ms
}

void calibrateSensors() {
  Serial.println("正在校准传感器...");
  float sum = 0;
  for(int i=0; i<100; i++){ // 取100次采样
    int16_t ax, ay, az;
    mpu.getAcceleration(&ax, &ay, &az);
    sum += az / 16384.0;
    delay(10);
  }
  baseline_z = sum / 100;
  Serial.print("初始基准值：");
  Serial.println(baseline_z);
}

void triggerAlarm() {
  digitalWrite(LED_PIN, HIGH);
  Serial.println("！！！跌倒检测！！！");
  
  // 报警闪烁模式
  for(int i=0; i<5; i++) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(300);
  }
  digitalWrite(LED_PIN, LOW);
}