/*******************************************************************************
 * 用陀螺仪控制小球移动
 * 本例中使用了OLED12864 和 MPU6050两个组件，但是OLED手里之后恶心的 SH1106
 * 这货跟 MPU6050 或者其他的 IIC 设备不太兼容，同在一个总线容易出问题，
 * 所以大家选择OLED的时候尽量用0.9寸的SSD1306。
 *
 * 还是已恶心的 SH1106为例，必须使用第二条IIC总线，所以这里扩展用到了TwoWire
 * ball_task 线程中负责SH1106的刷新，这个芯片刷新OLED非常慢，大概1帧/秒
 * mpu_task 负责采集陀螺仪的偏转角度，根据偏转角度大小设置小球每帧滑动步长 
 ******************************************************************************/
#include <Adafruit_MPU6050.h>
#include <U8g2lib.h>
#include <Wire.h>
#define OLED_SCL 16   // OLED 的IIC_SCL引脚
#define OLED_SDA 17   // OLED 的IIC_SDA引脚
#define MPU_SDA  41   // MPU6050 的IIC_SDA引脚
#define MPU_SCL  42   // MPU6050 的IIC_SCL引脚

#define BALL_SIZE 5   // 球的大小

float x=64 ,y=32;     // 球的中心点坐标
float xa=0, ya=0;     // XY 轴的滑动速度
float xc=0, yc=0;     // 校准后的X轴和Y轴，上电后第一件事就是校准

/*******************************************************************************
 * LED 显示任务
 * 屏幕用的是 SH1106，使用的是IIC_1 ，也就是默认的IIC，
 * 因为在ESP32中使用了GPIO矩阵设置IIC，所有理论上所有的IIC引脚都需要使用Wire.begin
 * 做初始化。OLED的SDC和SCL分别是16和17引脚
 ******************************************************************************/
void ball_task(void *param_t){
  // 初始化U8G2库和OLED
  U8G2_SH1106_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /*clock=*/OLED_SCL, /*data=*/OLED_SDA, /*reset=*/U8X8_PIN_NONE); 
  Wire.begin(OLED_SDA, OLED_SCL);   // 初始化 IIC 总线
  u8g2.begin();                     // 初始化OLED
  for(;;){
    u8g2.clearBuffer();					    // 每帧开始先清空缓冲区
    u8g2.drawFrame(0,0,u8g2.getWidth(), u8g2.getHeight());      // 画一个边框，已确定小球滚动的范围 
    x-=xa; y-=ya;                   // 根据偏转角度计算本帧的移动步长
    // 以下四句将小球控制在横轴0~127，纵轴0~63之间，也就是屏幕的大小
    x = x<BALL_SIZE?BALL_SIZE:x;
    x = x>127-BALL_SIZE?127-BALL_SIZE:x;
    y = y<BALL_SIZE?BALL_SIZE:y;
    y = y>63-BALL_SIZE?63-BALL_SIZE:y;
    u8g2.drawDisc((int)x, (int)y, BALL_SIZE, U8G2_DRAW_ALL);    // 画一个实体小球
    u8g2.sendBuffer();					                                // 显示内容，将缓冲区内容发送到OLED，这一步巨慢无比
    delay(1);                                                   // 让出CPU
  }
}

/*******************************************************************************
 * MPU6050 偏转角度获取
 * 为了给恶心的 SH1106 让路，可怜的 MPU6050 只能委屈使用IIC_1
 * 
 ******************************************************************************/
void mpu_task(void* param_t){
  // 初始化MPU6050
  Adafruit_MPU6050 mpu;
  TwoWire wire = TwoWire(1);        // 使用第二路IIC，第一路索引为0， 第二路索引为1 ，如果存在第三路，则索引为2
  wire.begin(MPU_SDA,MPU_SCL);      // 使用41和42引脚初始化第二路IIC
  
  // Wire.begin(MPU_SDA, MPU_SCL);
  if (mpu.begin(MPU6050_I2CADDR_DEFAULT,&wire)){      // 使用第二路IIC初始化 MPU6050
  // if (mpu.begin()){
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);     // 设置重力加速度的测量范围为±8G
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);          // 设置陀螺仪的测量范围为±500度/秒
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);       // 设置滤波器带宽
    }else{
      printf("MPU6050 initialization failed!\n");
      vTaskDelete(NULL);
    }
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;
  /* 先校准
   * 第一次获取MPU6050数据是为了校准用，将开机时候的角度作为默认角度
   * 后面发生角度偏转的时候都已初始角度为参考
   * 因为MPU6050的安装是XY相反的，所以需要对调以下
   */
  mpu.getEvent(&accel, &gyro, &temp);     // 获得偏转角度，加速度和温度
  xc = accel.acceleration.y;
  yc = accel.acceleration.x;
  for(;;){
    /* 开始循环获取角度偏转信息，偏转角度越大小球滑动越快
     * 获得偏转角度后需要和初始化的角度做补偿运算
     */
    mpu.getEvent(&accel, &gyro, &temp);
    xa = accel.acceleration.y - xc;
    ya = accel.acceleration.x - yc;
    delay(100);
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S3!");
  xTaskCreate(mpu_task, "MPU6050", 10240, NULL, 1, NULL);
  xTaskCreate(ball_task, "BALL", 10240, NULL, 1, NULL);

  vTaskDelete(NULL);    // 自宫
}

void loop() {
}