#include <Arduino_BMI270_BMM150.h> // IMU pour Arduino Nano 33 BLE Sense Rev2
#include <Servo.h>                 // Contrôle des servos

// Déclaration des objets servo
Servo servoPitch;  // Gouverne de profondeur
Servo servoRoll;   // Ailerons

// Pins de commande des servos
const int pitchPin = 9;
const int rollPin = 10;

// Objectif : vol horizontal
const float targetPitch = 0.0;
const float targetRoll = 0.0;

// Gains proportionnels (à ajuster pour ton planeur)
const float Kp_pitch = 10.0;
const float Kp_roll = 10.0;

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

  if (!IMU.begin()) {
    Serial.println("IMU non détecté !");
    while (1);
  }

  Serial.println("IMU initialisé.");

  // Attacher les servos aux broches PWM
  servoPitch.attach(pitchPin);
  servoRoll.attach(rollPin);
}

void loop() {
  float ax, ay, az;

  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(ax, ay, az);

    // Calcul des angles de roulis et tangage
    float roll  = atan2(ay, az) * 180.0 / PI;
    float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;

    // Calcul des erreurs
    float errorPitch = targetPitch - pitch;
    float errorRoll  = targetRoll - roll;

    // Calcul des commandes (contrôle P)
    float controlPitch = constrain(90 + Kp_pitch * errorPitch, 0, 180);
    float controlRoll  = constrain(90 + Kp_roll * errorRoll, 0, 180);

    // Envoi des commandes aux servos Fitec FS90
    servoPitch.write(controlPitch);
    servoRoll.write(controlRoll);

    // Affichage série pour débogage
    Serial.print("Pitch: "); Serial.print(pitch);
    Serial.print(" | Roll: "); Serial.print(roll);
    Serial.print(" | ServoPitch: "); Serial.print(controlPitch);
    Serial.print(" | ServoRoll: "); Serial.println(controlRoll);
  }

  delay(50); // Délai entre les cycles (réduction du bruit et de la fréquence)
}