#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <U8g2lib.h>

#define SCL 16
#define SDA 17

#define DISP_OLED 1
#define DISP_CONSOLE 1

typedef struct{
  // 温度
  float temp;

  // 加速度
  float acce_x;
  float acce_y;
  float acce_z;

  // 姿态
  float gyro_x;
  float gyro_y;
  float gyro_z;
}MPU6050;

MPU6050 mpu6050;
SemaphoreHandle_t xMutexMPU6050 = NULL;   // 同步锁
SemaphoreHandle_t xMutexIIC = NULL;            // IIC设备同步

Adafruit_MPU6050 mpu;   // 声明设备

// 陀螺仪任务
void mpu_task(void* param_t){
  if (xSemaphoreTake(xMutexIIC, 1000) == pdPASS) {
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);     // 设置重力加速度的测量范围为±8G
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);          // 设置陀螺仪的测量范围为±500度/秒
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);       // 设置滤波器带宽
    xSemaphoreGive(xMutexIIC);    // 释放同步锁
  }
  
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;
  
  for(;;){
    // 循环读取更新陀螺仪数据
    if (xSemaphoreTake(xMutexIIC, 1000) == pdPASS) {
      mpu.getEvent(&accel, &gyro, &temp);             // 获取数据
      xSemaphoreGive(xMutexIIC);    // 释放同步锁
    }
    if (xSemaphoreTake(xMutexMPU6050, 1000) == pdPASS) {
      // printf("获得钥匙，赶紧更新数据...\n");
      // 读出温度
      mpu6050.temp = temp.temperature;
      // 读出加速度
      mpu6050.acce_x = accel.acceleration.x / 9.81; // 得到的是重力， / 9.81得到的是牛
      mpu6050.acce_y = accel.acceleration.y / 9.81;
      mpu6050.acce_z = accel.acceleration.z / 9.81;
      // 读出姿态
      mpu6050.gyro_x = gyro.gyro.x *180 / PI;     // 得到的是弧度，转换为角度
      mpu6050.gyro_y = gyro.gyro.y *180 / PI;
      mpu6050.gyro_z = gyro.gyro.z *180 / PI;

      xSemaphoreGive(xMutexMPU6050);    // 释放同步锁
      // printf("数据更新完毕，把钥匙还回去了！\n");
    }else{
      printf("没人没送回钥匙来，我不等了~~\n");
    }
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

#if defined(DISP_CONSOLE) && DISP_CONSOLE == 1
// 打印输出线程
void print_task(void *param_t){
  for(;;){
    if (xSemaphoreTake(xMutexMPU6050, 1000) == pdPASS) {
      printf("-----===== MPU6050 =====-----\n");
      printf("Temp   : %.2f°C\n",mpu6050.temp);
      printf("Gyro-X : %.2f°\n",mpu6050.gyro_x);
      printf("Gyro-Y : %.2f°\n",mpu6050.gyro_y);
      printf("Gyro-Z : %.2f°\n",mpu6050.gyro_z);
      printf("Acce-X : %.2f g\n",mpu6050.acce_x);
      printf("Acce-Y : %.2f g\n",mpu6050.acce_y);
      printf("Acce-Z : %.2f g\n",mpu6050.acce_z);
      printf("==============================\n");
      xSemaphoreGive(xMutexMPU6050);
    }
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}
#endif

#if defined(DISP_OLED) && DISP_OLED == 1
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /*clock=*/SCL, /*data=*/SDA, /*reset=*/U8X8_PIN_NONE); 

// OLED显示线程
void led_task(void *param_t){
  if (xSemaphoreTake(xMutexIIC, 1000) == pdPASS) {
    u8g2.begin();
    // u8g2.setFont(u8g2_font_ncenB08_tr);	// 设置字体
    u8g2.setFont(u8g2_font_7x14_mr);
    xSemaphoreGive(xMutexIIC);       // 释放同步锁
  }
  char buf[24];   // 打印缓冲区
  for(;;){
    u8g2.clearBuffer();					// 清理缓冲区
    if (xSemaphoreTake(xMutexMPU6050, 1000) == pdPASS) {
      sprintf(buf,"Temp : %.2f°C\n",mpu6050.temp);
      u8g2.drawStr(0,10,buf);
      sprintf(buf,"GX:%3.2f° AX:%3.2fg\n",mpu6050.gyro_x, mpu6050.acce_x);
      u8g2.drawStr(0,25,buf);
      sprintf(buf,"GY:%3.2f° AY:%3.2fg\n",mpu6050.gyro_y, mpu6050.acce_y);
      u8g2.drawStr(0,40,buf);
      sprintf(buf,"GZ:%3.2f° AZ:%3.2fg\n",mpu6050.gyro_z, mpu6050.acce_z);
      u8g2.drawStr(0,55,buf);
      xSemaphoreGive(xMutexMPU6050);
    }

    if (xSemaphoreTake(xMutexIIC, 1000) == pdPASS) {
      u8g2.sendBuffer();					    // 显示内容
      xSemaphoreGive(xMutexIIC);       // 释放同步锁
    }
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}
#endif

void setup() {
  Serial.begin(115200);
  xMutexMPU6050 = xSemaphoreCreateMutex();    // 创建数据同步锁
  xMutexIIC = xSemaphoreCreateMutex();           // 创建IIC同步锁
  Wire.begin(SDA, SCL);   // 初始化I2C总线

  if (mpu.begin()) {      // 初始化陀螺仪
    Serial.println("MPU6050 初始化成功");
    xTaskCreatePinnedToCore(mpu_task, "MPU6050", 1024*4, NULL, 1, NULL, 1);  
#if defined(DISP_CONSOLE) && DISP_CONSOLE == 1
    xTaskCreate(print_task, "Print", 1024*4, NULL, 1, NULL);
#endif
#if defined(DISP_OLED) && DISP_OLED == 1
    xTaskCreatePinnedToCore(led_task, "OLED", 1024*4, NULL, 1, NULL,1);
#endif
  } else {
    Serial.println("MPU6050 初始化失败");
  }
}

void loop() {
  delay(10);
}