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

LiquidCrystal_I2C lcd(0x27,20,4);

Adafruit_MPU6050 mpu;
Adafruit_Sensor *mpu_temp,*mpu_accel,*mpu_gyro;

typedef struct{
  float temp;
  float accX;
  float accY;
  float accZ;
  float gyroX;
  float gyroY;
  float gyroZ;
}MPU6050;

MPU6050 mpu6050;

SemaphoreHandle_t xMutexMPU6050 = NULL;//创建信号量Handler
TickType_t timeOut = 1000;//用于获取信号量的Timeout 1000 ticks



void mpu6050Task(void *pvParam){
  mpu.begin();

  mpu_temp = mpu.getTemperatureSensor();
  mpu_temp->printSensorDetails();

  mpu_accel = mpu.getAccelerometerSensor();
  mpu_accel->printSensorDetails();

  mpu_gyro = mpu.getGyroSensor();
  mpu_gyro->printSensorDetails();

  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;

  while(1){
    if(xSemaphoreTake(xMutexMPU6050,timeOut) == pdPASS){

      //获取MPU 数据
      mpu_temp->getEvent(&temp);
      mpu_accel->getEvent(&accel);
      mpu_gyro->getEvent(&gyro);

      mpu6050.temp = temp.temperature;
      mpu6050.accX = accel.acceleration.x;
      mpu6050.accY = accel.acceleration.y;
      mpu6050.accZ = accel.acceleration.z;
      mpu6050.gyroX = gyro.gyro.x;
      mpu6050.gyroY = gyro.gyro.y;
      mpu6050.gyroZ = gyro.gyro.z;

      xSemaphoreGive(xMutexMPU6050);//释放钥匙
    }else{
      //Unable to obtain MUTEX
    }
    vTaskDelay(500);
  }
}

void lcdTask(void *ptParam)
{
  lcd.init();
  lcd.backlight();

  //定义是2004 LCD
  byte lcdline = 4;
  byte lcdChar = 20;

  //创建一个二维数组
  //注意长度是lcdChar+1 最后一个位置要给换行符
  char line0[lcdChar +1],line1[lcdChar +1],line2[lcdChar +1],line3[lcdChar +1];
  char *line[] = {line0,line1,line2,line3};

  while(1){
    if(xSemaphoreTake(xMutexMPU6050,timeOut) == pdPASS){

      //组合数据
      sprintf(line0,"    MPU6050 %d",xTaskGetTickCount()/100);
      sprintf(line1,"Temperature %.2f",mpu6050.temp);
      sprintf(line2,"ACC %.2f %.2f %.2f",mpu6050.accX,mpu6050.accY,mpu6050.accZ);
      sprintf(line3,"GYRO %.2f %.2f %.2f",mpu6050.gyroX,mpu6050.gyroY,mpu6050.gyroZ);

      xSemaphoreGive(xMutexMPU6050);//释放钥匙
    }else{
      //Unable to obtain MUTEX
    }
    //显示数据
    for(int i=0;i<4;i++){
      lcd.setCursor(0,i);
      lcd.print(line[i]);
    }
    vTaskDelay(1000);
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  xMutexMPU6050 = xSemaphoreCreateMutex();//创建MUTEX

  xTaskCreate(mpu6050Task,"MPU6050",1024*8,NULL,1,NULL);
  vTaskDelay(1000);//让MPU6050提前运行一秒获取第一笔数据
  xTaskCreate(lcdTask,"lcd",1024*8,NULL,1,NULL);
}

void loop() {
  // put your main code here, to run repeatedly:
  vTaskDelay(100); // this speeds up the simulation
}