// ---------------------------------------------------------------------------
//Libraries
#include <SPI.h>
#include <RF24.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <ESP32Servo.h>
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
//define debug macro
//#define DEBUG // Uncomment this line to debug

#ifdef DEBUG
  #define debug_begin() Serial.begin(115200)
  #define print(...) Serial.print(__VA_ARGS__)
  #define println(...) Serial.println(__VA_ARGS__)
#else
  #define debug_begin(...)
  #define print(...)
  #define println(...)
#endif
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
//define the pins used by the nrf24l01+pa+lna module
#define CS 5
#define CE 4
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
//configure the mpu6050 module
const byte receiver[6] = {'S', '1', '2', '3', '4', '\0'};
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
//define struct to send to receiver
struct Signal {
  byte throttle;
  byte pitch;
  byte roll;
  byte yaw;
  byte aux1;
  byte aux2;
  byte aux3;
  byte aux4;
};
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
// Customize here pulse lengths as needed (esc_calibration)
#define MIN_PULSE_LENGTH 1000  // Minimum pulse length in µs
#define MAX_PULSE_LENGTH 2000  // Maximum pulse length in µs
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
// Customize here servos pin
#define ENGINE_PIN 26
#define SXROLLER 16
#define DXROLLER 17
#define PITCHER 27
#define YAWER 14

Servo engine;
Servo rollersx;
Servo rollerdx;
Servo rudder;
Servo elevator;
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
//define global struct variable to send data
Signal data;
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
//function to reset struct data
void ResetData() {
  data.throttle = 100;
  data.pitch = 90;
  data.roll = 90;
  data.yaw = 90;
  data.aux1 = 90;
  data.aux2 = 90;
  data.aux3 = 90;
  data.aux4 = 90;
}
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
MPU6050 mpu6050(Wire);
RF24 radio(CE, CS);  // CE_PIN = 4, CSN_PIN = 5
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
void setup() {
  Serial.begin(115200);
  debug_begin();
  ResetData();
  //setup radio comunication
  if (!radio.begin()) {
    println(F("Il modulo radio non risponde!!"));
    while (0) {}
  }
  //radio.setPALevel(RF24_PA_LOW);
  radio.setPayloadSize(sizeof(Signal));
  // Ricevitore fisso
  radio.setChannel(104);
  radio.openReadingPipe(1, receiver);
  radio.startListening();
  //end of setup radio comunication


  //initializing rollers
  rollersx.attach(SXROLLER);
  rollerdx.attach(DXROLLER);
  //and testing them
  int time = 500;
  rollersx.write(45);
  delay(time);
  rollersx.write(135);
  delay(time);
  rollersx.write(90);
  rollerdx.write(135);
  delay(time);
  rollerdx.write(45);
  delay(time);
  rollersx.write(90);
  delay(time);
  rollersx.write(45);
  rollerdx.write(135);
  delay(time);
  rollersx.write(135);
  rollerdx.write(45);
  delay(time);
  rollersx.write(90);
  rollerdx.write(90);
  delay(time);
  rollersx.write(135);
  rollerdx.write(135);
  delay(time);
  rollersx.write(45);
  rollerdx.write(45);
  delay(time);
  rollersx.write(90);
  rollerdx.write(90);
  //end of the test



  //initializing elevator
  elevator.attach(PITCHER);
  //and testing it
  elevator.write(45);
  delay(time);
  elevator.write(135);
  delay(time);
  elevator.write(90);
  //end of the test


  //initializing rudder
  rudder.attach(YAWER);
  //and testing it
  rudder.write(45);
  delay(time);
  rudder.write(135);
  delay(time);
  rudder.write(90);
  //end of the test


  //calibrating esc
  engine.attach(ENGINE_PIN, MIN_PULSE_LENGTH, MAX_PULSE_LENGTH);
  engine.writeMicroseconds(MIN_PULSE_LENGTH);
  delay(200);
  engine.writeMicroseconds(MAX_PULSE_LENGTH);
  delay(200);
  engine.writeMicroseconds(MIN_PULSE_LENGTH);

  delay(time);
  // Inizializza il MPU6050
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets();

  delay(time);


  ResetData();


  println("Calibrated");
}
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
// Dichiarazione di una variabile per tenere traccia dell'ultimo tempo di lettura dalla radio
unsigned long lastRadioReadTime = millis();
// ---------------------------------------------------------------------------


// ---------------------------------------------------------------------------
void loop() {
  mpu6050.update();

  if (radio.available()) {
    radio.read(&data, sizeof(Signal));

    println(data.throttle);
    println(data.roll);
    println(data.pitch);
    println(data.yaw);

    // Imposta il tempo attuale come l'ultimo tempo di lettura dalla radio
    lastRadioReadTime = millis();
    Serial.println(F(lastRadioReadTime));

    //engine data
    if (100 <= data.throttle && data.throttle <= 200) {
      engine.write(map(data.throttle, 100, 200, 1000, 2000));
    } else {
      engine.write(1000);
    }

    //roll data
    if (0 <= data.roll && data.roll <= 180) {
      rollersx.write(data.roll);
      rollerdx.write(data.roll);
    }

    //pitch data
    if (0 <= data.pitch && data.pitch <= 180) {
      elevator.write(data.pitch);
    }

    //yaw data
    if (0 <= data.yaw && data.yaw <= 180) {
      rudder.write(data.yaw);
    }

  } else {
    // Verifica se sono passati più di 400 ms dall'ultima lettura dalla radio
    if (millis() - lastRadioReadTime > 400) {
      // Fai le stesse operazioni dell'else già presente

      // Calcola gli angoli di roll, pitch e yaw
      float roll = mpu6050.getAngleX();
      float pitch = mpu6050.getAngleY();
      float yaw = mpu6050.getAngleZ();

      // Controlla i limiti degli angoli se necessario
      roll = constrain(roll, -45, 45);
      pitch = constrain(pitch, -45, 45);
      yaw = constrain(yaw, -45, 45);

      // Mappa gli angoli negativi a valori positivi per i servo
      int rollServoValue = map(roll, -45, 45, 45, 135);
      int pitchServoValue = map(pitch, -45, 45, 45, 135);
      int yawServoValue = map(yaw, -45, 45, 135, 45);

      // Muovi i servo per stabilizzarlo
      engine.write(1000);
      rollersx.write(rollServoValue);
      rollerdx.write(rollServoValue);
      elevator.write(pitchServoValue);
      rudder.write(90);
    }
  }
}
// ---------------------------------------------------------------------------