#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#define CALIBRATION_DATA_VOLUME 3// 校准时读入多少个数据
#define ALPHA 0.9 // 定义一个常数，用于低通滤波器的权重

// 定义一个类，用于操作MPU6050
class MPU6050 {
 private:
    Adafruit_MPU6050 *myMpu;// 定义一个指针，指向Adafruit_MPU6050对象
    float base_x, base_y, base_z;// 定义一个变量，用于存储基准加速度方向
    float angle_threshold = 0.5;// 定义一个变量，用于设置夹角的阈值（单位为弧度）
    float filtered_x, filtered_y, filtered_z; // 定义一个变量，用于存储滤波后的加速度方向
  public:
    // 构造函数，传入一个Adafruit_MPU6050对象
    MPU6050(Adafruit_MPU6050 &mpu) {
      myMpu = &mpu;
    }

    // 初始化函数，返回是否成功
    bool begin() {
      // 初始化mpu模块
      if (!myMpu->begin()) {
        Serial.println("Failed to start MPU6050");
        return false;
      }
      // 设置加速度计量程为+-8G
      myMpu->setAccelerometerRange(MPU6050_RANGE_8_G);
      // 设置陀螺仪量程为+-500度/秒
      myMpu->setGyroRange(MPU6050_RANGE_500_DEG);
      // 设置滤波器带宽为21Hz
      myMpu->setFilterBandwidth(MPU6050_BAND_21_HZ);
      // 调用calibrate函数，计算并存储基准加速度方向
      calibrate();
      return true;
    }

    // 检测函数，返回是否倒地
    bool detect() {
      // 获取加速度计数据
      sensors_event_t a, g, temp;
      myMpu->getEvent(&a, &g, &temp);
      // 计算加速度向量和基准向量的夹角（单位为弧度）
      float angle = acos((a.acceleration.x * base_x + a.acceleration.y * base_y + a.acceleration.z * base_z) / (sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z) * sqrt(base_x * base_x + base_y * base_y + base_z * base_z)));
      // 判断是否倒地
      if (angle > angle_threshold) {
        Serial.println("yes!");
        return true;
      } else {
        Serial.println("No!");
        return false;
      }
    }

 

    // 定义一个函数，用于计算并存储基准加速度方向
    void calibrate() {
      // 初始化base_x, base_y, base_z为0
      base_x = 0;
      base_y = 0;
      base_z = 0;

      // 获取CALIBRATION_DATA_VOLUM次加速度计数据，每次间隔0.1s，并累加到base_x, base_y, base_z中
      for (int i = 0; i < CALIBRATION_DATA_VOLUME; i++) {
        sensors_event_t a, g, temp;
        myMpu->getEvent(&a, &g, &temp);
        base_x += a.acceleration.x;
        base_y += a.acceleration.y;
        base_z += a.acceleration.z;
        delay(100);
      }

      // 计算base_x, base_y, base_z的平均值，并存储在base_x, base_y, base_z中
      base_x /= CALIBRATION_DATA_VOLUME;
      base_y /= CALIBRATION_DATA_VOLUME;
      base_z /= CALIBRATION_DATA_VOLUME;
    }

    bool detecte(){
        sensors_event_t a, g, temp;
        myMpu->getEvent(&a, &g, &temp);
        // 计算加速度向量和基准向量的夹角（单位为弧度）
        float angle = acos((a.acceleration.x * base_x + a.acceleration.y * base_y + a.acceleration.z * base_z) / (sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z) * sqrt(base_x * base_x + base_y * base_y + base_z * base_z)));
        // 判断是否倒地
        if (angle > angle_threshold) {
          Serial.println("Fall detected");
          return 1;
        } else {
          Serial.println("Stand detected");
          return 0;
        }
    }
};




Adafruit_MPU6050 mpu;
MPU6050 myMpu(mpu);




void setup() {
  Serial.begin(115200);
  myMpu.begin();
}

void loop() {
  // 获取加速度计数据
  myMpu.detecte();
  delay(500);
}