// #include <IBusBM.h>
#include <ESP32Servo.h>
// #include <Preferences.h>
#include <Wire.h>  // Include library for I2C communication

// --- MOTOR CONTROL VARIABLES ---
// IBusBM ibus;
Servo esc1, esc2, esc3, esc4;
// Preferences prefs;

// ESC output pins
#define ESC1_PIN 13
#define ESC2_PIN 12
#define ESC3_PIN 14
#define ESC4_PIN 27

// Throttle range for ESCs
const int MIN_THROTTLE = 1000;
const int MAX_THROTTLE = 2000;

// ARM and dispatch control
#define ARM_CHANNEL 6      // Switch to arm/disarm motors
#define DOOR_CHANNEL 5     // Switch to open the door
#define DOOR_SERVO_PIN 26  // Attach your servo to this pin
Servo doorServo;
bool isArmed = false;
unsigned long channelsPrintTimer = 0;

// Timing control
unsigned long lastUpdate = 0;
const unsigned long escInterval = 20;  // 20ms -> 50Hz

// Configuration
#define THROTTLE_CHANNEL 2  // Change this to match your transmitter
struct ChannelConfig {
  int min_val;
  int max_val;
  int center;
  int deadzone;
} config[4];

enum CalState { IDLE,
                STEP1,
                STEP2
              };
CalState calState = IDLE;
unsigned long calibrationStart;

// --- FLIGHT CONTROL LOGIC VARIABLES ---
// Timing variables for LED blinking and gyro reading
#define LED 2  // Onboard LED pin (ESP32 DevKit)
unsigned long ledPrevMillis = 0;
const unsigned long ledInterval = 1500;  // Blink every 1.5 seconds
bool ledState = LOW;

unsigned long gyroPrevMillis = 0;
const unsigned long imuInterval = 10;  // IMU (Gyro & Acc) read interval: 10ms (100Hz)
float imuDt = imuInterval * 0.001;     // convert ms to seconds | imuInterval = 10ms; dt = 0.010s

// Variables to store gyro rates and calibration offsets
float RateRoll, RatePitch, RateYaw;
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;

// Variables to store acc. calibration offsets
float CalibrationAccX, CalibrationAccY, CalibrationAccZ;

uint32_t LoopTimer;
uint32_t loopStartMillis;
unsigned int ArmSwitch;

float DesiredRateRoll, DesiredRatePitch, DesiredRateYaw;
float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw;
float InputRoll, InputThrottle, InputPitch, InputYaw;
float PrevErrorRateRoll, PrevErrorRatePitch, PrevErrorRateYaw;
float PrevItermRateRoll, PrevItermRatePitch, PrevItermRateYaw;

float PIDReturn[] = { 0, 0, 0 };
float PRateRoll = 0.6, PRatePitch = PRateRoll, PRateYaw = 2;
float IRateRoll = 3.5, IRatePitch = IRateRoll, IRateYaw = 12;
float DRateRoll = 0.03, DRatePitch = DRateRoll, DRateYaw = 0;
float MotorInput1, MotorInput2, MotorInput3, MotorInput4;

// Variables to store accelerometer data and calculated angles
float AccX, AccY, AccZ;
float AngleRoll, AnglePitch;

float KalmanAngleRoll = 0, KalmanUncertaintyAngleRoll = 2 * 2;
float KalmanAnglePitch = 0, KalmanUncertaintyAnglePitch = 2 * 2;
float Kalman1DOutput[] = { 0, 0 };

float DesiredAngleRoll, DesiredAnglePitch;
float ErrorAngleRoll, ErrorAnglePitch;
float PrevErrorAngleRoll, PrevErrorAnglePitch;
float PrevItermAngleRoll, PrevItermAnglePitch;
float PAngleRoll = 2;
float PAnglePitch = PAngleRoll;
float IAngleRoll = 0;
float IAnglePitch = IAngleRoll;
float DAngleRoll = 0;
float DAnglePitch = DAngleRoll;

float AccZInertial;
float AltitudeKalman, VelocityVerticalKalman;
float DesiredVelocityVertical, ErrorVelocityVertical;
float PVelocityVertical = 3.5;
float IVelocityVertical = 0.0015;
float DVelocityVertical = 0.01;
float PrevErrorVelocityVertical, PrevItermVelocityVertical;

void setup() {
  Serial.begin(115200);
  Serial.println("\nHi Boss!");
  // // digitalWrite(LED, HIGH);      // Turn on LED
  pinMode(LED, OUTPUT);  // Initialize LED pin

  // loadConfig();

  Wire.setClock(400000);  // Set I2C speed to 400kHz (fast mode)
  Wire.begin();           // Start I2C communication
  delay(250);

  // // Initialize iBUS on Serial2 (GPIO16/RX2)
  // ibus.begin(Serial2, IBUSBM_NOTIMER, 16, 17);  // RX=16, TX=17

  // // Attach ESCs
  esc1.attach(ESC1_PIN, MIN_THROTTLE, MAX_THROTTLE);
  esc2.attach(ESC2_PIN, MIN_THROTTLE, MAX_THROTTLE);
  esc3.attach(ESC3_PIN, MIN_THROTTLE, MAX_THROTTLE);
  esc4.attach(ESC4_PIN, MIN_THROTTLE, MAX_THROTTLE);

  start_mpu6050();      // Initialize MPU6050 sensor
  calibrate_mpu6050();  // Calibrate gyro offsets

  // // Arm ESCs
  armESCs();

  // Serial.println("System Ready!");
  // // hRemoteCalibration();
  delay(3500);
  // waitForSafeThrottle();
}

void loop() {
  printf(".");
  loopStartMillis = millis();
  LoopTimer = loopStartMillis;
  // ibus.loop();  // Process iBUS data

  // hRemoteCalibration();

  // Arm/Disarm logic
  // int armSwitch = ibus.readChannel(ARM_CHANNEL);
  // isArmed = (armSwitch > 1500);  // Adjust threshold if needed
  isArmed = (ArmSwitch > 1500);  // Adjust threshold if needed

  // if(Serial.available()) printChannelsEverySecond();  // Print every second

  // // Door control logic
  // int doorSwitch = ibus.readChannel(DOOR_CHANNEL);
  // if (doorSwitch > 1500) {
  //   doorServo.write(90);  // Open door
  // } else {
  //   doorServo.write(0);   // Close door
  // }

  // Blink LED at defined interval
  if (loopStartMillis - ledPrevMillis >= ledInterval) {
    ledPrevMillis = loopStartMillis;
    blink_led();
  }

  // Read gyro and accelerometer data at 100Hz , 10ms
  if (loopStartMillis - gyroPrevMillis >= imuInterval) {
    gyroPrevMillis = loopStartMillis;
    calibrated_imu_signals();  // Read and calibrate gyro data
    print_gyro();              // Output sensor data to serial
  }

  // if(calState == IDLE) {
  // Moter update at 20ms
  if (loopStartMillis - lastUpdate >= escInterval) {
    lastUpdate = loopStartMillis;

    analogJoystickToPWM();
    if (isArmed) {
    updateMotors();  // Only run motors if armed
    } else {
      // Motors disarmed — send min throttle to all ESCs
      esc1.writeMicroseconds(MIN_THROTTLE);
      esc2.writeMicroseconds(MIN_THROTTLE);
      esc3.writeMicroseconds(MIN_THROTTLE);
      esc4.writeMicroseconds(MIN_THROTTLE);
    }

  }
  // }

}

void analogJoystickToPWM() {
  const int yaw = 34;
  const int throttle = 35;
  const int pitch = 32;
  const int roll = 33;
  int yaw_adc_val, throttle_adc_val, pitch_adc_val, roll_adc_val;
  int yaw_pwm, throttle_pwm, pitch_pwm, roll_pwm;
  yaw_adc_val = analogRead(yaw);
  throttle_adc_val = analogRead(throttle);
  pitch_adc_val = analogRead(pitch);
  roll_adc_val = analogRead(roll);

  ArmSwitch = map(analogRead(16), 0, 1023, 1000, 2000);
  InputThrottle = map(analogRead(throttle), 0, 1023, 1000, 2000);

  yaw_pwm = map(yaw_adc_val, 0, 4095, 1000, 2000) ;  /*Convert digital value to voltage */
  // throttle_pwm = map(throttle_adc_val, 0, 4095, 1000, 2000)  ;  /*Convert digital value to voltage */
  pitch_pwm = map(pitch_adc_val, 0, 4095, 1000, 2000)  ;  /*Convert digital value to voltage */
  roll_pwm = map(roll_adc_val, 0, 4095, 1000, 2000)  ;  /*Convert digital value to voltage */


  DesiredAngleRoll = roll_pwm - 1500;
  DesiredAnglePitch = pitch_pwm - 1500;
  // InputThrottle = throttle_pwm;
  DesiredRateYaw = yaw_pwm - 1500;

}

// Core Functions
// Wait until throttle is in safe range before allowing motor start
// void waitForSafeThrottle() {
//   int throttleValue;
//   do {
//     ibus.loop();
//     throttleValue = ibus.readChannel(THROTTLE_CHANNEL);
//     delay(4);  // Small delay to avoid excessive looping
//   } while (throttleValue < 1020 || throttleValue > 1050);
// }

// Blink onboard LED
void blink_led() {
  ledState = !ledState;
  digitalWrite(LED, ledState);
}

// Initialize MPU6050 sensor settings (called once in setup)
void start_mpu6050() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);  // Power management register
  Wire.write(0x00);  // Wake up MPU6050 (clear sleep bit)
  Wire.endTransmission();

  // Configure Digital Low Pass Filter (DLPF) to ~10Hz bandwidth
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);  // CONFIG register
  Wire.write(0x05);  // Set DLPF_CFG = 5
  Wire.endTransmission();

  // Set gyroscope full-scale range to ±500°/s
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);  // GYRO_CONFIG register
  Wire.write(0x08);  // Set FS_SEL = 1 (±500 dps)
  Wire.endTransmission();

  // Set accelerometer full-scale range to ±8g
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);  // ACCEL_CONFIG register
  Wire.write(0x10);  // Set AFS_SEL = 2 (±8g)
  Wire.endTransmission();
}

// Applies a 1D Kalman filter to fuse gyro and accelerometer data for angle estimation
void kalman_1d(float angleEstimate, float uncertainty, float gyroRate, float accelAngle) {

  // float dt = imuInterval * 0.001;  // convert ms to seconds | imuInterval = 10ms; dt = 0.010

  // Step 1: Predict the new angle using gyro input (rate × time)
  // angleEstimate = angleEstimate + 0.004 * gyroRate;  // 0.004 sec = 4 ms sample time
  // angleEstimate = angleEstimate + dt * gyroRate;
  angleEstimate = angleEstimate + imuDt * gyroRate;

  // Step 2: Increase uncertainty due to process noise (gyro drift etc.)
  // uncertainty = uncertainty + dt * dt * 4 * 4;  // Q = (dt² × noise²)
  uncertainty = uncertainty + imuDt * imuDt * 4 * 4;  // Q = (dt² × noise²)

  // Step 3: Compute Kalman Gain (balance trust between gyro and accelerometer)
  float kalmanGain = uncertainty / (uncertainty + 3 * 3);  // R = accel noise²

  // Step 4: Correct estimate using accelerometer measurement
  angleEstimate = angleEstimate + kalmanGain * (accelAngle - angleEstimate);

  // Step 5: Update uncertainty after measurement correction
  uncertainty = (1 - kalmanGain) * uncertainty;

  // Output: Store filtered angle and its uncertainty
  Kalman1DOutput[0] = angleEstimate;
  Kalman1DOutput[1] = uncertainty;
}

// Read raw gyro data from MPU6050
void gyro_signals(void) {
  // --- Read Gyroscope Data ---
  Wire.beginTransmission(0x68);
  Wire.write(0x43);  // Gyro_XOUT_H register
  Wire.endTransmission();
  Wire.requestFrom(0x68, 6);
  while (Wire.available() < 6)
    ;  // Wait for all bytes
  int16_t GyroX = Wire.read() << 8 | Wire.read();
  int16_t GyroY = Wire.read() << 8 | Wire.read();
  int16_t GyroZ = Wire.read() << 8 | Wire.read();

  // Convert raw gyro data to degrees per second
  RateRoll = (float)GyroX / 65.5;
  RatePitch = (float)GyroY / 65.5;
  RateYaw = (float)GyroZ / 65.5;
}

void pid_equation(float Error, float P, float I, float D, float PrevError, float PrevIterm) {
  // Convert interval from milliseconds to seconds
  // float dt = imuInterval * 0.001f;

  // Proportional term
  float Pterm = P * Error;

  // Integral term using trapezoidal integration
  // float Iterm = PrevIterm + I * (Error + PrevError) * dt / 2.0f;
  float Iterm = PrevIterm + I * (Error + PrevError) * imuDt / 2;
  Iterm = constrain(Iterm, -400, 400);  // Anti-windup clamping

  // Derivative term
  // float Dterm = D * (Error - PrevError) / dt;
  float Dterm = D * (Error - PrevError) / imuDt;

  // Final PID output
  float PIDOutput = Pterm + Iterm + Dterm;
  PIDOutput = constrain(PIDOutput, -400, 400);  // Output clamping

  // Store result in global array
  PIDReturn[0] = PIDOutput;  // Output value
  PIDReturn[1] = Error;      // Store current error for next cycle
  PIDReturn[2] = Iterm;      // Store updated Iterm for next cycle
}

void reset_pid(void) {
  PrevErrorRateRoll = 0;
  PrevErrorRatePitch = 0;
  PrevErrorRateYaw = 0;

  PrevItermRateRoll = 0;
  PrevItermRatePitch = 0;
  PrevItermRateYaw = 0;

  PrevErrorAngleRoll = 0;
  PrevErrorAnglePitch = 0;

  PrevItermAngleRoll = 0;
  PrevItermAnglePitch = 0;
}

//Read raw accelerometer data from MPU6050
void acc_signals() {
  // --- Read Accelerometer Data ---
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);  // ACCEL_XOUT_H register
  Wire.endTransmission();
  Wire.requestFrom(0x68, 6);
  while (Wire.available() < 6)
    ;  // Wait for all bytes
  int16_t AccXLSB = Wire.read() << 8 | Wire.read();
  int16_t AccYLSB = Wire.read() << 8 | Wire.read();
  int16_t AccZLSB = Wire.read() << 8 | Wire.read();

  // Convert raw accelerometer data to g (gravity)
  // AccX = (float)AccXLSB / 4096.0 - 0.02;
  // AccY = (float)AccYLSB / 4096.0 + 0.01;
  // AccZ = (float)AccZLSB / 4096.0 + 0.01;
  AccX = (float)AccXLSB / 4096.0;
  AccY = (float)AccYLSB / 4096.0;
  AccZ = (float)AccZLSB / 4096.0;
}

// Calibrate gyro offsets by averaging multiple samples at rest
void calibrate_mpu6050() {
  Serial.println("Don't move! Calibrating Gyro Sensor...");
  delay(1500);

  const int MaxGyroSamples = 250;
  RateCalibrationRoll = 0;
  RateCalibrationPitch = 0;
  RateCalibrationYaw = 0;

  for (int i = 0; i < MaxGyroSamples; i++) {
    gyro_signals();
    RateCalibrationRoll += RateRoll;
    RateCalibrationPitch += RatePitch;
    RateCalibrationYaw += RateYaw;
    delay(4);  // Wait 4ms between samples (~500Hz sampling) << 1KHz
    Serial.printf("Gyro [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f\n", RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw);
  }

  // Calculate average offsets
  RateCalibrationRoll /= MaxGyroSamples;
  RateCalibrationPitch /= MaxGyroSamples;
  RateCalibrationYaw /= MaxGyroSamples;

  // Serial.printf("Calibrated Offsets [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f\n",
  //               RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw);

  const int MaxAccSamples = 100;
  CalibrationAccX = 0;
  CalibrationAccY = 0;
  CalibrationAccZ = 0;

  for (int i = 0; i < MaxAccSamples; i++) {
    acc_signals();
    CalibrationAccX += AccX;
    CalibrationAccY += AccY;
    CalibrationAccZ += 1 - AccZ;  // As initially the drone is place Upright!
    delay(2);                     // Wait 2ms between samples (~500Hz sampling) << 1KHz
    Serial.printf("Acc [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f\n", CalibrationAccX, CalibrationAccY, CalibrationAccZ);
  }

  // Calculate average offsets
  CalibrationAccX /= MaxAccSamples;
  CalibrationAccY /= MaxAccSamples;
  CalibrationAccZ /= MaxAccSamples;

  Serial.printf("Calibrated Gyro Offsets [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f\n",
                RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw);
  Serial.printf("Calibrated Acc Offsets [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f\n",
                CalibrationAccX, CalibrationAccY, CalibrationAccZ);
}

// Read gyro data and subtract calibration offsets
void calibrated_imu_signals() {
  gyro_signals();
  RateRoll -= RateCalibrationRoll;
  RatePitch -= RateCalibrationPitch;
  RateYaw -= RateCalibrationYaw;

  acc_signals();
  AccX -= CalibrationAccX;
  AccY -= CalibrationAccY;
  AccZ -= CalibrationAccZ;

  float degToRad = 3.14159265 / 180;

  // Calculate roll and pitch angles from accelerometer data (degrees)
  AngleRoll = atan(AccY / sqrt(AccX * AccX + AccZ * AccZ)) * (1 / degToRad);
  AnglePitch = -atan(AccX / sqrt(AccY * AccY + AccZ * AccZ)) * (1 / degToRad);

  // Applying 1D Kalman Filter
  kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
  KalmanAngleRoll = Kalman1DOutput[0];
  KalmanUncertaintyAngleRoll = Kalman1DOutput[1];
  kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
  KalmanAnglePitch = Kalman1DOutput[0];
  KalmanUncertaintyAnglePitch = Kalman1DOutput[1];

  // Acceleration in Z-Axis
  AccZInertial = -sin(AnglePitch * degToRad) * AccX + cos(AnglePitch * degToRad) * sin(AngleRoll * degToRad) * AccY + cos(AnglePitch * degToRad) * cos(AngleRoll * degToRad) * AccZ;
  AccZInertial = (AccZInertial - 1) * 9.81 * 100;
}


// Print accelerometer and gyro data to serial monitor
void print_gyro() {
  // Serial.print("Roll rate [°/s]= ");
  // Serial.print(RateRoll);
  // Serial.print(" Pitch Rate [°/s]= ");
  // Serial.print(RatePitch);
  // Serial.print(" Yaw Rate [°/s]= ");
  // Serial.println(RateYaw);

  // Serial.print("Acceleration X [g]= ");
  // Serial.print(AccX);
  // Serial.print(" Acceleration Y [g]= ");
  // Serial.print(AccY);
  // Serial.print(" Acceleration Z [g]= ");
  // Serial.println(AccZ);

  // Serial.printf("Gyro [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f\n", RateRoll, RatePitch, RateYaw);
  // Serial.printf("Accel [g] → X: %.2f Y: %.2f Z: %.2f | Gyro [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f\n", AccX, AccY, AccZ, RateRoll, RatePitch, RateYaw);

  // Serial.print("Roll Angle [°] ");
  // Serial.print(KalmanAngleRoll);
  // Serial.print(" Pitch Angle [°] ");
  // Serial.println(KalmanAnglePitch);

  // Serial.printf("Kalman Roll: %.2f, Pitch: %.2f | Accel [g] → X: %.2f Y: %.2f Z: %.2f | Gyro [°/s] → Roll: %.2f Pitch: %.2f Yaw: %.2f \n",KalmanAngleRoll, KalmanAnglePitch, AccX, AccY, AccZ, RateRoll, RatePitch, RateYaw);
  Serial.printf("Kalman Roll: %+6.2f, Pitch: %+6.2f | Accel [g] → X: %+5.2f Y: %+5.2f Z: %+5.2f | Gyro [°/s] → Roll: %+6.2f Pitch: %+6.2f Yaw: %+6.2f | Raw Angle → Roll: %+6.2f Pitch: %+6.2f\n",
                KalmanAngleRoll, KalmanAnglePitch,
                AccX, AccY, AccZ,
                RateRoll, RatePitch, RateYaw,
                AngleRoll, AnglePitch);
  // delay(50);  // Delay for readability (20Hz print rate)
}

// void printChannelsEverySecond() {
//   if (millis() - channelsPrintTimer > 1000) {
//     channelsPrintTimer = millis();
//     for (int i = 0; i < 10; i++) {
//       Serial.print("CH");
//       Serial.print(i);
//       Serial.print(": ");
//       Serial.print(ibus.readChannel(i));
//       Serial.print(" ");
//     }
//     Serial.println();
//   }
// }

void armESCs() {
  esc1.writeMicroseconds(MIN_THROTTLE);
  esc2.writeMicroseconds(MIN_THROTTLE);
  esc3.writeMicroseconds(MIN_THROTTLE);
  esc4.writeMicroseconds(MIN_THROTTLE);
  // delay(1000);
  Serial.println("ESCs Armed");
}

void updateMotors() {
  // Read channels (adjust numbers to match your TX)
  // float roll = readCalibratedChannel(0);                     // Left X-Stick (Horizontal)
  // float pitch = readCalibratedChannel(1);                    // Right Y-Stick (Vertical)
  // float throttle = readCalibratedChannel(THROTTLE_CHANNEL);  // Right Y-Stick (Vertical)
  // float yaw = readCalibratedChannel(3);                      // Right X-Stick (Horizontal)
  // float roll = ibus.readChannel(0);   // Left X-Stick (Horizontal)
  // float pitch = ibus.readChannel(1);  // Right Y-Stick (Vertical)
  // float throttle = ibus.readChannel(THROTTLE_CHANNEL);  // Right Y-Stick (Vertical)
  // float yaw = ibus.readChannel(3);  // Right X-Stick (Horizontal)
  // float aux1 = ibus.readChannel(4);
  // float aux2 = ibus.readChannel(5);
  // int sw1 = ibus.readChannel(6);
  // float sw2 = ibus.readChannel(7);
  // float sw3 = ibus.readChannel(8);
  // float sw4 = ibus.readChannel(9);

  // DesiredAngleRoll = 0.10 * (roll - 1500);    // from +50 to -50 degree
  // DesiredAnglePitch = 0.10 * (pitch - 1500);  // from +50 to -50 degree
  // InputThrottle = ibus.readChannel(THROTTLE_CHANNEL);
  // DesiredRateYaw = 0.15 * (yaw - 1500);  // from +75 to -75 degree

  // DesiredAngleRoll = roll - 1500;
  // DesiredAnglePitch = pitch - 1500;
  // InputThrottle = ibus.readChannel(THROTTLE_CHANNEL);
  // DesiredRateYaw = yaw - 1500;

  DesiredVelocityVertical = 0.3 * (InputThrottle - 1500);
  ErrorVelocityVertical = DesiredVelocityVertical - VelocityVerticalKalman;
  pid_equation(ErrorVelocityVertical, PVelocityVertical, IVelocityVertical, DVelocityVertical, PrevErrorVelocityVertical, PrevItermVelocityVertical);
  float _InputThrottle = 1500 + PIDReturn[0];
  PrevErrorVelocityVertical = PIDReturn[1];
  PrevItermVelocityVertical = PIDReturn[2];

  ErrorAngleRoll = DesiredAngleRoll - KalmanAngleRoll;
  ErrorAnglePitch = DesiredAnglePitch - KalmanAnglePitch;

  pid_equation(ErrorAngleRoll, PAngleRoll, IAngleRoll, DAngleRoll, PrevErrorAngleRoll, PrevItermAngleRoll);
  DesiredRateRoll = PIDReturn[0];
  PrevErrorAngleRoll = PIDReturn[1];
  PrevItermAngleRoll = PIDReturn[2];
  pid_equation(ErrorAnglePitch, PAnglePitch, IAnglePitch, DAnglePitch, PrevErrorAnglePitch, PrevItermAnglePitch);
  DesiredRatePitch = PIDReturn[0];
  PrevErrorAnglePitch = PIDReturn[1];
  PrevItermAnglePitch = PIDReturn[2];

  // ErrorRateRoll = DesiredAngleRoll - KalmanAngleRoll;
  // ErrorRatePitch = DesiredAnglePitch - KalmanAnglePitch;
  ErrorRateRoll = DesiredRateRoll - RateRoll;
  ErrorRatePitch = DesiredRatePitch - RatePitch;
  ErrorRateYaw = DesiredRateYaw - RateYaw;

  pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, DRateRoll, PrevErrorRateRoll, PrevItermRateRoll);
  InputRoll = PIDReturn[0];
  PrevErrorRateRoll = PIDReturn[1];
  PrevItermRateRoll = PIDReturn[2];
  pid_equation(ErrorRatePitch, PRatePitch, IRatePitch, DRatePitch, PrevErrorRatePitch, PrevItermRatePitch);
  InputPitch = PIDReturn[0];
  PrevErrorRatePitch = PIDReturn[1];
  PrevItermRatePitch = PIDReturn[2];
  pid_equation(ErrorRateYaw, PRateYaw, IRateYaw, DRateYaw, PrevErrorRateYaw, PrevItermRateYaw);
  InputYaw = PIDReturn[0];
  PrevErrorRateYaw = PIDReturn[1];
  PrevItermRateYaw = PIDReturn[2];

  // InputRoll = ErrorRateRoll;
  // InputPitch = ErrorRatePitch;
  // InputYaw = ErrorRateYaw;

  if (InputThrottle > 1800) InputThrottle = 1800;

  // printf("InputThrottle: %.2f, InputRoll:%.2f, InputPitch:%.2f, InputYaw:%.2f \n", InputThrottle, InputRoll, InputPitch, InputYaw);
  MotorInput1 = (InputThrottle - InputRoll - InputPitch - InputYaw);  // 1.024 *
  MotorInput2 = (InputThrottle - InputRoll + InputPitch + InputYaw);  // 1.024 *
  MotorInput3 = (InputThrottle + InputRoll + InputPitch - InputYaw);  // 1.024 *
  MotorInput4 = (InputThrottle + InputRoll - InputPitch + InputYaw);  // 1.024 *

  int ThrottleIdle = 1180;
  if (MotorInput1 < ThrottleIdle) MotorInput1 = ThrottleIdle;
  if (MotorInput2 < ThrottleIdle) MotorInput2 = ThrottleIdle;
  if (MotorInput3 < ThrottleIdle) MotorInput3 = ThrottleIdle;
  if (MotorInput4 < ThrottleIdle) MotorInput4 = ThrottleIdle;

  // ThrottleCutOff
  if (InputThrottle < 1050) {
    MotorInput1 = MotorInput2 = MotorInput3 = MotorInput4 = MIN_THROTTLE;
    reset_pid();
  }
  // if (ArmSwitch < 1500) {
  //   MotorInput1 = MotorInput2 = MotorInput3 = MotorInput4 = MIN_THROTTLE;
  // }

  esc1.writeMicroseconds(MotorInput1);
  esc2.writeMicroseconds(MotorInput2);
  esc3.writeMicroseconds(MotorInput3);
  esc4.writeMicroseconds(MotorInput4);

  Serial.printf("Thr: %+6.2f Y: %+6.2f P: %+6.2f R: %+6.2f | M1: %+6.2f M2: %+6.2f M3: %+6.2f M4: %+6.2f | KR: %+6.2f, KP: %+6.2f | AccZI: %+6.2f, _IThr: %+6.2f | Sw1: %4d \n",
                InputThrottle, InputYaw, InputPitch, InputRoll,
                MotorInput1, MotorInput2, MotorInput3, MotorInput4,
                KalmanAngleRoll, KalmanAnglePitch, AccZInertial, _InputThrottle, ArmSwitch);

  // int m1_pwm, m2_pwm, m3_pwm, m4_pwm;

  // if (sw1 < 1500) {
  //   m1_pwm = m2_pwm = m3_pwm = m4_pwm = MIN_THROTTLE;
  // } else {
  //   // Quadcopter X-config mixing
  //   float m1 = throttle + pitch + roll - yaw;
  //   float m2 = throttle + pitch - roll + yaw;
  //   float m3 = throttle - pitch + roll + yaw;
  //   float m4 = throttle - pitch - roll - yaw;

  //   // Convert back to PWM (1000-2000µs) with constrain 0–1 range
  //   m1_pwm = scaleToPWM(m1);
  //   m2_pwm = scaleToPWM(m2);
  //   m3_pwm = scaleToPWM(m3);
  //   m4_pwm = scaleToPWM(m4);
  // }

  // // // Write to ESCs
  // esc1.writeMicroseconds(m1_pwm);
  // esc2.writeMicroseconds(m2_pwm);
  // esc3.writeMicroseconds(m3_pwm);
  // esc4.writeMicroseconds(m4_pwm);

  // // Debug output
  // Serial.printf("Thr: %.2f Y: %.2f P: %.2f R: %.2f | M1: %4d M2: %4d M3: %4d M4: %4d | Sw1: %4d\n", throttle, yaw, pitch, roll, m1_pwm, m2_pwm, m3_pwm, m4_pwm, sw1);
}

// Calibration System
// void hRemoteCalibration() {
//   Serial.println("Send 'c' to calibrate (within 5 seconds)...");
//   // delay(5000);
//   unsigned long waitStart = millis();
//   bool calibrationRequested = false;

//   while (millis() - waitStart < 5000) {  // Wait for 5 seconds
//     if (Serial.available()) {
//       char ch = Serial.read();
//       if (ch == 'c' || ch == 'C') {
//         calibrationRequested = true;
//         break;
//       }
//     }
//     delay(10);
//   }

//   if (calibrationRequested) {
//     Serial.println("Calibration initiated!");
//     startCalibration();  // Call your calibration function or set calibration state
//     // hRemoteCalibration();
//   } else {
//     Serial.println("No calibration requested, proceeding with normal startup.");
//   }
//   // if(Serial.available()) {
//   //   char cmd = Serial.read();
//   //   if(cmd == 'c' && calState == IDLE) startCalibration();
//   // }
// }
// void startCalibration() {
//   calState = STEP1;
//   Serial.println("\n=== CALIBRATION ===");
//   Serial.println("1. Set throttle to MIN, center other sticks");
//   Serial.println("Press any key when ready...");
//   while (!Serial.available())
//     ;
//   Serial.read();

//   ibus.loop();
//   // Record throttle min and centers
//   config[THROTTLE_CHANNEL].min_val = ibus.readChannel(THROTTLE_CHANNEL);
//   for (int i = 0; i < 4; i++) {
//     if (i == THROTTLE_CHANNEL) continue;
//     config[i].center = ibus.readChannel(i);
//     config[i].min_val = 1500;
//     config[i].max_val = 1500;

//     Serial.printf("\nCh: %d, Center: %d", i, ibus.readChannel(i));
//   }
//   delay(3000);
//   calState = STEP2;
//   runCalibrationStep2();
// }

// void runCalibrationStep2() {
//   calibrationStart = millis();
//   Serial.println("\n2. Move ALL sticks through full range for 10 seconds");
//   delay(4000);
//   Serial.println("Calibrating...");
//   // Update min/max in real-time
//   while (millis() - calibrationStart <= 10000) {
//     ibus.loop();
//     for (int i = 0; i < 4; i++) {
//       int raw = ibus.readChannel(i);

//       if (raw < config[i].min_val) config[i].min_val = raw;
//       else if (raw > config[i].max_val) config[i].max_val = raw;

//       Serial.printf("\n2. Ch: %d, Min: %d, Max: %d", i, config[i].min_val, config[i].max_val);
//     }
//     // delay(100);
//   }


//   // for(int i=0; i<4; i++) {
//   //   int raw = ibus.readChannel(i);

//   //   if(i == THROTTLE_CHANNEL) {
//   //     if(raw > config[i].max_val) config[i].max_val = raw;
//   //   } else {
//   //     if(raw < config[i].min_val) config[i].min_val = raw;
//   //     if(raw > config[i].max_val) config[i].max_val = raw;
//   //   }
//   // }

//   // Check calibration time
//   // if(millis() - calibrationStart >= 3000) {
//   finalizeCalibration();
//   calState = IDLE;
//   // }
// }

// void finalizeCalibration() {
//   // Calculate centers and deadzones
//   for (int i = 0; i < 4; i++) {
//     if (i != THROTTLE_CHANNEL) {
//       // config[i].center = (config[i].min_val + config[i].max_val) / 2;
//       config[i].deadzone = (config[i].max_val - config[i].min_val) * 0.03;
//     }
//   }

//   saveConfig();
//   Serial.println("\nCalibration Complete!");
//   printConfig();

//   Serial.printf("\nLoop Starts in 15sec");
//   delay(15000);
// }
// Helper Functions
// int mapPWM(int raw, int in_min, int in_max) {
//   // Prevent division by zero
//   if (in_max == in_min) return 1500;
//   // Linear mapping, floating point for precision
//   float scaled = (float)MIN_THROTTLE + (raw - in_min) * (float)MIN_THROTTLE / (in_max - in_min);
//   // Constrain to 1000-2000
//   if (scaled < MIN_THROTTLE) return MIN_THROTTLE;
//   if (scaled > MAX_THROTTLE) return MAX_THROTTLE;
//   return (int)scaled;
// }

// float readCalibratedChannel(int ch) {
//   int raw = ibus.readChannel(ch);
//   raw = mapPWM(raw, config[ch].min_val, config[ch].max_val);

//   if (ch == THROTTLE_CHANNEL) {
//     return constrain((raw - 1000) / 1000.0, 0.0, 1.0);
//   }

//   if (abs(raw - config[ch].center) < config[ch].deadzone) return 0.0;

//   return constrain((raw - 1500) / 1000.0, -1.0, 1.0);
// }

// int scaleToPWM(float value) {
//   value = constrain(value, 0.0, 1.0);
//   return (value * 1000) + 1000;
// }

// void loadConfig() {
//   prefs.begin("tx_config", true);
//   for (int i = 0; i < 4; i++) {
//     config[i].min_val = prefs.getInt(("ch" + String(i) + "_min").c_str(), 1000);
//     config[i].max_val = prefs.getInt(("ch" + String(i) + "_max").c_str(), 2000);
//     config[i].center = prefs.getInt(("ch" + String(i) + "_cen").c_str(), 1500);
//     config[i].deadzone = prefs.getInt(("ch" + String(i) + "_dz").c_str(), 20);
//   }
//   prefs.end();
// }

// void saveConfig() {
//   prefs.begin("tx_config", false);
//   for (int i = 0; i < 4; i++) {
//     prefs.putInt(("ch" + String(i) + "_min").c_str(), config[i].min_val);
//     prefs.putInt(("ch" + String(i) + "_max").c_str(), config[i].max_val);
//     prefs.putInt(("ch" + String(i) + "_cen").c_str(), config[i].center);
//     prefs.putInt(("ch" + String(i) + "_dz").c_str(), config[i].deadzone);
//   }
//   prefs.end();
// }

// void printConfig() {
//   Serial.println("Stored Configuration:");
//   for (int i = 0; i < 4; i++) {
//     Serial.printf("CH%d: min=%4d max=%4d center=%4d deadzone=%3d\n",
//                   i, config[i].min_val, config[i].max_val,
//                   config[i].center, config[i].deadzone);
//   }
// }