#include "Wire.h"
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_10DOF.h>

#include <Servo.h>

#include <PID_v1.h>

/************************************************************************/
/**************************VARIABLE RADIO********************************/
/************************************************************************/
int vitesse = 0;

const int pinCh1 = A0;
const int pinCh2 = A1;
const int pinCh3 = A2;
const int pinCh4 = A3;
const int pinCh5 = A4;

//micros lors du HIGH
volatile unsigned long timer_start1;
volatile unsigned long timer_start2;
volatile unsigned long timer_start3;
volatile unsigned long timer_start4;
volatile unsigned long timer_start5;

//difference entre les périodes
volatile int pulse_time1;
volatile int pulse_time2;
volatile int pulse_time3;
volatile int pulse_time4;
volatile int pulse_time5;

//Pour mapper les commandes entre 0 et 100
const int minCh1 = 1190;
const int minCh2 = 1160;
const int minCh3 = 1137;
const int minCh4 = 1125;
const int minCh5 = 9999;

const int maxCh1 = 1930;
const int maxCh2 = 1810;
const int maxCh3 = 1790;
const int maxCh4 = 1890;
const int maxCh5 = 9999;

double ch1 = 0;
double ch2 = 0;
double ch3 = 0;
double ch4 = 0;
double ch5 = 0;

volatile int last_interrupt_time1;
volatile int last_interrupt_time2;
volatile int last_interrupt_time3;
volatile int last_interrupt_time4;
volatile int last_interrupt_time5;



/************************************************************************/
/**************************VARIABLES IMU*********************************/
/************************************************************************/

/* Assign a unique ID to the sensors */
Adafruit_10DOF dof = Adafruit_10DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(18001);

/* Update this with the correct SLP for accurate altitude measurements */
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;

double pitchKi = 0.5;
double pitchKp = 0;
double pitchKd = 1;

double rollKi = 2;
double rollKp = 5;
double rollKd = 1;

double yawKi = 2;
double yawKp = 5;
double yawKd = 1;


double pitchIN;
double rollIN;
double yawIN;

double pitchOUT;
double rollOUT;
double yawOUT;

//IN,OUT,SETPOINT
PID pidPitch(&pitchIN, &pitchOUT, &ch2, pitchKp, pitchKi, pitchKd, DIRECT);
PID pidRoll(&rollIN, &rollOUT, &ch1, rollKp, rollKi, rollKd, DIRECT);
PID pidYaw(&yawIN, &yawOUT, &ch4, yawKp, yawKi, yawKd, DIRECT);

int pitchMin = -30;
int pitchMax = 30;

int rollMin = -30;
int rollMax = 30;

int yawMin = -180;
int yawMax = 180;



/************************************************************************/
/**************************VARIABLES MOTORS******************************/
/************************************************************************/

//Objet servo pour controler les diff�rents moteurs
Servo motor1, motor2, motor3, motor4;

const int pinM1 = 9;
const int pinM2 = 10;
const int pinM3 = 11;
const int pinM4 = 6;

int speedM1 = 0;
int speedM2 = 0;
int speedM3 = 0;
int speedM4 = 0;
int speedM1M3 = 0;
int speedM2M4 = 0;

const int  motorSpeedMin = 0;
const int motorSpeedMax = 170;


/************************************************************************/
/***********************************MAIN*********************************/
/************************************************************************/

void setup()
{
  Serial.begin(115200);
  initSensors();
  initInterrupt();
  initPID();
  initMotors(); // 10 SECONDES

}

void loop()
{
  getSensor();
  getRadio();//map les cannaux de 0 � 100
  calcSpeed();//calcul en fontion des commandes et du PID la vitesse � appliquer aux moteurs
  updateMotors();//met � jour la vitesse des moteurs

  /// TEST_affichagePitch();
  // TEST_affichageCanaux();
  TEST_affichageMoteur();

}


/************************************************************************/
/****************************FONCTIONS RADIO*****************************/
/************************************************************************/
void isr1()
{
  //Enregistre la dernière interruption
  last_interrupt_time1 = micros();

  //Si high
  if (digitalRead(pinCh1) == HIGH)
  {
    timer_start1 = micros();
  }
  //Si low
  else
  {
    //timer déclenché?
    if (timer_start1 > 0)
    {
      //Enregristre le temps
      pulse_time1 = ((volatile int)micros() - timer_start1);
      //redémarre le timer
      timer_start1 = 0;
    }
  }
}

void isr2()
{
  last_interrupt_time2 = micros();
  if (digitalRead(pinCh2) == HIGH) {
    timer_start2 = micros();
  }
  else {
    if (timer_start2 > 0) {
      pulse_time2 = ((volatile int)micros() - timer_start2);
      timer_start2 = 0;
    }
  }
}

void isr3()
{
  last_interrupt_time3 = micros();
  if (digitalRead(pinCh3) == HIGH) {
    timer_start3 = micros();
  }
  else {
    if (timer_start3 > 0) {
      pulse_time3 = ((volatile int)micros() - timer_start3);
      timer_start3 = 0;
    }
  }
}

void isr4()
{
  last_interrupt_time4 = micros();
  if (digitalRead(pinCh4) == HIGH) {
    timer_start4 = micros();
  }
  else {
    if (timer_start4 > 0) {
      pulse_time4 = ((volatile int)micros() - timer_start4);
      timer_start4 = 0;
    }
  }
}

void isr5()
{
  last_interrupt_time5 = micros();
  if (digitalRead(pinCh5) == HIGH) {
    timer_start5 = micros();
  }
  else {
    if (timer_start5 > 0) {
      pulse_time5 = ((volatile int)micros() - timer_start5);
      timer_start5 = 0;
    }
  }
}

void initInterrupt() {
  timer_start1 = 0;
  timer_start2 = 0;
  timer_start3 = 0;
  timer_start4 = 0;
  timer_start5 = 0;
  attachInterrupt(pinCh1, isr1, CHANGE);
  attachInterrupt(pinCh2, isr2, CHANGE);
  attachInterrupt(pinCh3, isr3, CHANGE);
  attachInterrupt(pinCh4, isr4, CHANGE);
  attachInterrupt(pinCh5, isr5, CHANGE);
}

void getRadio() {
  ch1 = map(pulse_time1, minCh1, maxCh1, rollMin, rollMax); //ROLL
  ch2 = map(pulse_time2, minCh2, maxCh2, pitchMin, pitchMax); //PITCH
  ch3 = map(pulse_time3, minCh3, maxCh3, 0, 100); //VITESSE
  ch4 = map(pulse_time4, minCh4, maxCh4, yawMin, yawMax); //YAW
  ch5 = map(pulse_time5, minCh5, maxCh5, 0, 100);
}

void TEST_affichageCanaux() {
  Serial.print("CH1 : ");
  Serial.print(pulse_time1);
  Serial.print("-->");
  Serial.print(ch1);
  Serial.print(" - CH2 : ");
  Serial.print(pulse_time2);
  Serial.print("-->");
  Serial.print(ch2);
  Serial.print(" - CH3 : ");
  Serial.print(pulse_time3);
  Serial.print("-->");
  Serial.print(ch3);
  Serial.print(" - CH4 : ");
  Serial.print(pulse_time4);
  Serial.print("-->");
  Serial.println(ch4);
}


/************************************************************************/
/***************************FONCTIONS IMU********************************/
/************************************************************************/
/* Code fourni par AdaFruit */
void initSensors() {
  if (!accel.begin()) {
    Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
    while (1);
  }
  if (!mag.begin()) {
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while (1);
  }
  if (!bmp.begin()) {
    Serial.println("Ooops, no BMP180 detected ... Check your wiring!");
    while (1);
  }
}

void getSensor() {
  sensors_event_t accel_event;
  sensors_event_t mag_event;
  sensors_event_t bmp_event;
  sensors_vec_t orientation;

  /* Calculate pitch and roll from the raw accelerometer data */
  accel.getEvent(&accel_event);
  if (dof.accelGetOrientation(&accel_event, &orientation)) {
 
    rollIN = map(orientation.roll, -180, 180, -30, 30);
    pitchIN = map(orientation.pitch, -180, 180, -30, 30);
  }
  /* Calculate the heading using the magnetometer */
  mag.getEvent(&mag_event);
  if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
    
    yawIN = map(orientation.heading, -180, 180, -30, 30);
  }
  /* Calculate the altitude using the barometric pressure sensor */
  bmp.getEvent(&bmp_event);
  if (bmp_event.pressure) {
    /* Get ambient temperature in C */
    float temperature;
    bmp.getTemperature(&temperature);
    }
}


void TEST_affichagePitch() {
  Serial.print("Yaw:");
  Serial.print(yawIN);
  Serial.print(" - Kp:");
  Serial.print(pitchKp);
  Serial.print(" - Ki:");
  Serial.print(pitchKi);
  Serial.print(" - Kd:");
  Serial.print(pitchKd);
  Serial.print(" - CH2:");
  Serial.print(ch2);
  Serial.print(" - PitchIN:");
  Serial.print(pitchIN);
  Serial.print(" - PitchOUT:");
  Serial.println(pitchOUT);
}

void TEST_affichageRoll() {
  Serial.print("RollIN : ");
  Serial.print(rollIN);
  Serial.print(" - RollOUT : ");
  Serial.println(rollOUT);
}

void TEST_affichageYaw() {
  Serial.print("YawIN : ");
  Serial.print(yawIN);
  Serial.print(" - YawOUT : ");
  Serial.println(yawOUT);
}
/************************************************************************/
/***************************FONCTION CALCUL*****************************/
/************************************************************************/
void initPID() {
  pidPitch.SetMode(AUTOMATIC);
  pidPitch.SetOutputLimits(-20, 20);
  pidRoll.SetMode(AUTOMATIC);
  pidRoll.SetOutputLimits(-20, 20);
  pidYaw.SetMode(AUTOMATIC);
  pidYaw.SetOutputLimits(-180, 180);
}


void calcSpeed() {
  noInterrupts();
  pidPitch.Compute();
  pidRoll.Compute();
  pidYaw.Compute();
  interrupts();

   speedM1 = map(constrain(ch3 + pitchOUT, 0, 100), 0, 100, motorSpeedMin, motorSpeedMax);
    speedM3 = map(constrain(ch3 - pitchOUT, 0, 100), 0, 100, motorSpeedMin, motorSpeedMax);
     speedM2 = map(constrain(ch3 + rollOUT, 0, 100), 0, 100, motorSpeedMin, motorSpeedMax);
    speedM4 = map(constrain(ch3 - rollOUT, 0, 100), 0, 100, motorSpeedMin, motorSpeedMax);

  //>>>Appliquer le YAW
}


void TEST_affichageMoteur() {
  Serial.print("M1 :");
  Serial.print(speedM1);
  Serial.print(" - M2:");
  Serial.print(speedM2);
  Serial.print(" - M3:");
  Serial.print(speedM3);
  Serial.print(" - M4:");
  Serial.println(speedM4);
}
/************************************************************************/
/***************************FONCTIONS MOTORS*****************************/
/************************************************************************/

//Initialisation des moteurs : 10 SECONDES
void initMotors() {
  Serial.println("InitMotors");

  motor1.attach(pinM1);
  motor2.attach(pinM2);
  motor3.attach(pinM3);
  motor4.attach(pinM4);
  motor1.write(170);
  motor2.write(170);
  motor3.write(170);
  motor4.write(170);
  Serial.println("Brancher ALIM!!");
  delay(5000);
  motor1.write(0);
  motor2.write(0);
  motor3.write(0);
  motor4.write(0);
  delay(5000);
}

//Mise a jour de la vitesse des moteur en fontion des variables globales
void updateMotors() {
  vitesse = map(ch3, 0, 100, 0, 160);
  motor1.write(vitesse);
  motor2.write(vitesse);
  motor3.write(vitesse);
  motor4.write(vitesse);


}