#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>              
#include <MadgwickAHRS.h>      // 四元數計算歐拉角
#include <QuickPID.h>          // 雙環計算
#include <BluetoothSerial.h>

// 設定採樣頻率 (Hz)
unsigned long lastMillis = 0;   // 創建計時器
const float sampleFreq = 100.0; // 採樣頻率(Hz)
const float interval = 1000.0 / sampleFreq;

// 設定陀螺儀當前數據
float now_roll = 0.0;
float now_yaw = 0.0;
float now_pitch = 0.0;
// 馬達變化值宣告
float finalpitch;
float finalroll;
// 陀螺儀校準數值
float main_pitch = 0.0;
float main_roll = 0.0;
// 陀螺儀目標數值
float target_pitch;
float target_roll;
// PID調校數值
float Kp = 1.2, Ki = 0.5, Kd = 0.1;

// 馬達預設
int motor = 1000;
int motor_updown = 0;
int m1; // 左前    m1---m2
int m2; // 右前     |    |
int m3; // 左後     |    |
int m4; // 右後    m3---m4

// 設定裝置
Adafruit_MPU6050 mpu;
Madgwick filter;

// 設定雙環
QuickPID pitchPID(&now_pitch, &finalpitch, &target_pitch);
QuickPID rollPID(&now_roll, &finalroll, &target_roll);


// 初始化陀螺儀
void resetmpu6050(){
  float sum_roll = 0.0;
  float sum_pitch = 0.0;

  for(int i = 0 ; i < 100 ; i++){
    update_data();
    sum_roll += now_roll;
    sum_pitch += now_pitch;
  }
  main_roll = sum_roll/100;
  main_pitch = sum_pitch/100;
  target_roll = main_roll;
  target_pitch = main_pitch;
}

// 初始化PID函式
void setupPID(){
  int range = 400; // 按鍵靈敏度(修正範圍)
  
  pitchPID.SetTunings(Kp, Ki, Kd);
  pitchPID.setOutputLimits((range * -1), range);
  pitchPID.SetMode(QuickPID::Control::Automatic);

  rollPID.SetTunings(Kp, Ki, Kd);
  rollPID.SetOutputLimits((range * -1), range);
  rollPID.SetMode(QuickPID::Control::Automatic);

}

// 更新陀螺儀數據
void update_data(){
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Madgwick 濾波器需要傳入：
  // 1. 陀螺儀數據：必須轉換為 度/秒 (Degrees/s)
  // 2. 加速度數據：單位為 G 或 m/s^2 (此處使用 m/s^2)
  // 注意：Madgwick 庫的 updateIMU 接收的陀螺儀單位通常是度/秒
   
  float gx = g.gyro.x * 57.2958; // 弧度轉度 (rad/s -> deg/s)
  float gy = g.gyro.y * 57.2958;
  float gz = g.gyro.z * 57.2958;

  // 更新濾波器 (不含磁力計使用 updateIMU)
  filter.updateIMU(gx, gy, gz, a.acceleration.x, a.acceleration.y, a.acceleration.z);

  // 取得歐拉角 (單位：度) 
  /*
  roll  -> 左右翻滾
  pitch -> 前傾後仰
  yaw   -> 面朝方向
  */

  now_roll = filter.getRoll();
  now_pitch = filter.getPitch();
  now_yaw = filter.getYaw();
}

// 根據指令行動
void command(char cmd){
  // 上升下降
  if(cmd == 'u'){
    motor_updown = 200; // 增加升力
    return;
  }else if(cmd == 'd'){
    motor_updown = -200; // 減少升力
    return;
  }else{
    motor_updown = 0;
  }

  // 左右轉
  if(cmd == 'l'){
    target_roll -= 200;
    return;
  }else if(cmd == 'r'){
    target_roll += 200;
    return;
  }

  // 前後
  if(cmd == 'f'){
    target_pitch -= 10;
    return;
  }else if(cmd == 'b'){
    target_pitch += 10;
  }

}

// 更新馬達轉速
void update_motor(){
  m1 = constrain(motor + finalpitch + finalroll + motor_updown, 1000, 2000); // 左前    m1---m2
  m2 = constrain(motor + finalpitch - finalroll + motor_updown, 1000, 2000); // 右前     |    |
  m3 = constrain(motor - finalpitch + finalroll + motor_updown, 1000, 2000); // 左後     |    |
  m4 = constrain(motor - finalpitch - finalroll + motor_updown, 1000, 2000); // 右後    m3---m4
}

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

  // 初始化 MPU6050
  if (!mpu.begin()) {
    Serial.println("找不到 MPU6050！");
    while (1) yield();
  }
  // 初始化 Madgwick 濾波器頻率
  filter.begin(sampleFreq);
  resetmpu6050();
  
  // 初始化rollPID&pitchPID
  setupPID();
  Serial.println("MPU6050 + Madgwick 濾波器已啟動");
}



void loop() {
  // 確保以固定的頻率執行濾波運算
  if (millis() - lastMillis >= interval) { // 計時
    lastMillis = millis();
    
    // 更新歐拉角
    update_data();

    // 讀取輸入
    if (SerialBT.available()) {
      char cmd = SerialBT.read(); 
      command(cmd);
    }

    // 計算PID
    pitchPID.compute();
    rollPID.compute();

    // 更新馬達狀態
    update_motor();

    // 印出結果
    Serial.print("Roll: "); Serial.print(now_roll);
    Serial.print(" \tPitch: "); Serial.print(now_pitch);
    Serial.print(" \tYaw: "); Serial.println(now_yaw);
  }
}