 const float pi = 3.14159265359;
 Adafruit_BNO055 bno = Adafruit_BNO055();
 
-//// Fonctions de lecture du gyro et accel
-
-//Initiation du IMU
-void bno_initialisation() {
+
+
+
+// Fonction qui s'execute une fois et au début
+void setup() {
+  Serial.begin(9600);
+  //BNO init
   bno.begin();
   bno.setExtCrystalUse(true);
-}
-
-// Calculer inclinaison
-void bno_get_values() {
+  
+  //RX init
+  attachInterrupt(A0, calcSignal0, CHANGE);
+  attachInterrupt(A1, calcSignal1, CHANGE);
+  attachInterrupt(A2, calcSignal2, CHANGE);
+  attachInterrupt(A3, calcSignal3, CHANGE);
+  attachInterrupt(A4, calcSignal4, CHANGE);
+
+  //Motors init
+  esc_1.attach(MOTOR_PIN_FRONT_RIGHT);
+  esc_2.attach(MOTOR_PIN_FRONT_LEFT);
+  esc_3.attach(MOTOR_PIN_BACK_LEFT);
+  esc_4.attach(MOTOR_PIN_BACK_RIGHT);
+}
+
+
+// Fonction qui s'execute a chaque cycle
+void loop() {
+  
+  //IMU calculations
   if (auto_stabilisation_mode == false) {
     imu::Vector<3> gyroscope = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
     roll_speed = gyroscope.x() * 180 / pi;
     pitch_speed = gyroscope.y() * 180 / pi;
     yaw_speed = gyroscope.z() * 180 / pi;
-    pid_roll_speed_in = roll_speed;
-    pid_pitch_speed_in = pitch_speed;
-    pid_yaw_speed_in = yaw_speed;
-  }
-  else {
+  }else{
     imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
     roll_angle = euler.z();
     pitch_angle = euler.y();
-    pid_roll_angle_in = map(roll_angle, -180, 180, ROLL_WMIN, ROLL_WMAX);
-    pid_pitch_angle_in = map(pitch_angle, -180, 180, PITCH_WMIN, PITCH_WMAX);
-  }
-}
-
-
-//// Fonctions de controle
-
-// Calculer l'output et l'ecrire
-void control_update() {
-  // Fait correspondre les valeurs du joystick aux valeurs des moteurs
+    yaw_angle = euler.x();
+  }
+
+
+  //MOTORS
   throttle = map(input[2], THROTTLE_RMIN, THROTTLE_RMAX, MOTOR_ZERO_LEVEL, MOTOR_MAX_LEVEL);
-
-  // Calculer les valeurs a écrire aux moteurs
+  //PID
   pid_compute();
-
   // Ecrire les valeurs aux moteurs
   motorFR = throttle + pid_pitch_out + pid_roll_out - pid_yaw_out;
   motorFL = throttle + pid_pitch_out - pid_roll_out + pid_yaw_out;

  esc_2.writeMicroseconds(MOTOR_ZERO_LEVEL);
  esc_4.writeMicroseconds(MOTOR_ZERO_LEVEL);
}

//// Fonctions radio

//TODO: fonction qui lit les interrupts pour une execution plus rapide(pas encore)
void rx_initialize() {
  attachInterrupt(A0, calcSignal0, CHANGE);
  attachInterrupt(A1, calcSignal1, CHANGE);
  attachInterrupt(A2, calcSignal2, CHANGE);
  attachInterrupt(A3, calcSignal3, CHANGE);
}

// Fonctions qui lisent les valeurs de la radio
void calcSignal0()
{
  last_interrupt_time0 = micros();

  if (digitalRead(A0) == HIGH)
  {
    chrono_start0 = micros();
  }
  else
  {
    if (chrono_start0 != 0)
    {
      input[0] = ((volatile int)micros() - chrono_start0);
      chrono_start0 = 0;
    }
  }
}

void calcSignal1()
{
  last_interrupt_time1 = micros();

  if (digitalRead(A1) == HIGH)
  {
    chrono_start1 = micros();
  }
  else
  {
    if (chrono_start1 != 0)
    {
      input[1] = ((volatile int)micros() - chrono_start1);
      chrono_start1 = 0;
    }
  }
}

void calcSignal2()
{
  last_interrupt_time2 = micros();

  if (digitalRead(A2) == HIGH)
  {
    chrono_start2 = micros();
  }
  else
  {
    if (chrono_start2 != 0)
    {
      input[2] = ((volatile int)micros() - chrono_start2);
      chrono_start2 = 0;
    }
  }
}

void calcSignal3()
{
  last_interrupt_time3 = micros();

  if (digitalRead(A3) == HIGH)
  {
    chrono_start3 = micros();
  }
  else
  {
    if (chrono_start3 != 0)
    {
      input[3] = ((volatile int)micros() - chrono_start3);
      chrono_start3 = 0;
    }
  }
}


// Fonction qui s'execute une fois et au début
void setup() {
  Serial.begin(9600);
  bno_initialisation();
  rx_initialize();
  esc_1.attach(MOTOR_PIN_FRONT_RIGHT);
  esc_2.attach(MOTOR_PIN_FRONT_LEFT);
  esc_3.attach(MOTOR_PIN_BACK_LEFT);
  esc_4.attach(MOTOR_PIN_BACK_RIGHT);
}

// Fonctions de debug
void print_motors() {
  Serial.print(motorFR);
  Serial.print(",");

  Serial.println(input[3]);
}

