int PWMGaMin=48; //44
int PWMGaMax=215; //216
int PWMGaEcart;

int PWMDrMin=48;
int PWMDrMax=215;
int PWMDrEcart;

int PWMAvMin=48;
int PWMAvMax=215;//193
int PWMAvEcart;

int PWMArMin=48;//60
int PWMArMax=215;
int PWMArEcart;



// If you want to use PWM, you can use channel 5+6 on Rx directly. Then swap with other channels in PPM output
// To use channel 9 as PPM channel 5, det SWAPCH5 to 9. Channel 5 will be on channel 9 in PPM stream
#define SWAPCH5 8   // channel 9 mapped to channel 5 and the other way around
#define SWAPCH6 9  // channel 10 mapped to channel 6 and t1222he other way around

#define IBUS_MAXCHANNELS 14
// Failsafe values for the channels. FS-i6 does not support throttle failsafe < 1000, so we must fake it
// Setup failsafe on the FS-i6 to have all 6 channels to to -100 or thereabout to allow us to reliable detect it.
// This array must be minimum PPM_CHANS long
// In the example, below, I set channel 3 to 950 (Throttle) and channel 5 to 2000 (flight mode). Then just
// ensure RTH is on highest value on the flightmode switch, so even if FC does not see failsafe, it will enter RTH mode.
// We do no swapping in this array, so it goes directly to PPM
static uint16_t rcFailsafe[IBUS_MAXCHANNELS] = {  1500, 1500, 950, 1500, 2000, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
#define FAILSAFELIMIT 1020    // When all the 6 channels below this value assume failsafe

#define IBUS_BUFFSIZE 32    // Max iBus packet size (2 byte header, 14 channels x 2 bytes, 2 byte checksum)
                                                                                                                                     
static uint16_t rcValue[IBUS_MAXCHANNELS];
static uint16_t rcValueSafe[IBUS_MAXCHANNELS]; // read by interrupt handler. Data copied here in cli/sei block
static boolean rxFrameDone;
static boolean failsafe = 0;
uint16_t dummy;


static boolean rcReceived = 0 ;

uint16_t oldGazValue = 50; //Pourcentage, 50 % = arret, 100% = AV, 0% = AR
uint16_t oldDirectionValue = 50; //Pourcentage, de la droite vers la gauche;
uint16_t oldGlobalModeRcValue = 0; 

uint16_t gazValue = 50; //Pourcentage, 50 % = arret, 100% = AV, 0% = AR
uint16_t directionValue = 50; //Pourcentage, de la droite vers la gauche;
uint16_t globalMode = 1;
 
// Prototypes
void setupRx();
void readRx();
void setupPwm();
void analyseRC();
void appliMotorSpeed();
void showInformations();

void setup() {

  setupRx();
  setupPwm();
  Serial.println("Init complete");

}

void loop() {
  readRx();  
  if (rcReceived == 1)
  {
    analyseRC();
    appliMotorSpeed();
    showInformations();
    rcReceived = 0;    
  }
}

static uint8_t ibusIndex = 0;
static uint8_t ibus[IBUS_BUFFSIZE] = {0};

void setupRx()
{
  uint8_t i;
  Serial1.begin(115200);
  Serial.begin (115200);
  Serial.println ("coucou");
}

void setupPwm()
{
  pinMode(7, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(4, OUTPUT);
  
  PWMGaEcart = PWMGaMax - PWMGaMin;
  PWMDrEcart = PWMDrMax - PWMDrMin;
  PWMAvEcart = PWMAvMax - PWMAvMin;
  PWMArEcart = PWMArMax - PWMArMin;
  
  TCCR0B = TCCR0B & 0b11111000 | 0b001;
}

void readRx()
{
  uint8_t i;
  uint16_t chksum, rxsum;

  rxFrameDone = false;

  uint8_t avail = Serial1.available();
  
  if (avail)
  {
    //digitalWrite(4, LOW);
    uint8_t val = Serial1.read();
    // Look for 0x2040 as start of packet
    if (ibusIndex == 0 && val != 0x20) {
      return;
    }
    if (ibusIndex == 1 && val != 0x40) {
      ibusIndex = 0;
      return;
    }
 
    if (ibusIndex < IBUS_BUFFSIZE) ibus[ibusIndex] = val;
    ibusIndex++;

    if (ibusIndex == IBUS_BUFFSIZE)
    {
      ibusIndex = 0;
      chksum = 0xFFFF;
      for (i = 0; i < 30; i++)
        chksum -= ibus[i];

      rxsum = ibus[30] + (ibus[31] << 8);
      if (chksum == rxsum)
      {
        //Unrolled loop  for 10 channels - no need to copy more than needed.
        // MODIFY IF MORE CHANNELS NEEDED
        rcValue[0] = (ibus[ 3] << 8) + ibus[ 2];
        rcValue[1] = (ibus[ 5] << 8) + ibus[ 4];
        rcValue[2] = (ibus[ 7] << 8) + ibus[ 6];
        rcValue[3] = (ibus[ 9] << 8) + ibus[ 8];
        rcValue[4] = (ibus[11] << 8) + ibus[10];
        rcValue[5] = (ibus[13] << 8) + ibus[12];
        rcValue[6] = (ibus[15] << 8) + ibus[14];
        rcValue[7] = (ibus[17] << 8) + ibus[16];
        rcValue[8] = (ibus[19] << 8) + ibus[18];
        rcValue[9] = (ibus[21] << 8) + ibus[20];
        rxFrameDone = true;
        if (rcValue[0] < FAILSAFELIMIT && rcValue[1] < FAILSAFELIMIT &&
            rcValue[2] < FAILSAFELIMIT && rcValue[3] < FAILSAFELIMIT &&
            rcValue[4] < FAILSAFELIMIT && rcValue[5] < FAILSAFELIMIT ) 
        {
          failsafe = 1;
          cli(); // disable interrupts
          memcpy(rcValueSafe, rcFailsafe, IBUS_MAXCHANNELS * sizeof(uint16_t));
          sei();
          //digitalWrite(13, HIGH);  //  Error - turn on error LED
          Serial.println("ERREUR");
        }
        else
        {
          // Now we need to disable interrupts to copy 16-bit values atomicly
          // Only copy needed signals (10 channels default)
          // MODIFY IF MORE CHANNELS NEEDED
          cli(); // disable interrupts.
          rcValueSafe[0] = rcValue[0];
          rcValueSafe[1] = rcValue[1];
          rcValueSafe[2] = rcValue[2];
          rcValueSafe[3] = rcValue[3];
          rcValueSafe[4] = rcValue[SWAPCH5-1];
          rcValueSafe[5] = rcValue[SWAPCH6-1];
          rcValueSafe[6] = rcValue[6];
          rcValueSafe[7] = rcValue[7];
          rcValueSafe[8] = rcValue[8];
          rcValueSafe[9] = rcValue[9];
#if (SWAPCH5 != 5)
          rcValueSafe[SWAPCH5-1] = rcValue[4];
#endif
#if (SWAPCH6 != 6)
          rcValueSafe[SWAPCH6-1] = rcValue[5];
#endif          
          sei();
          //digitalWrite(13, LOW); // OK packet - Clear error LED                   

          rcReceived = 1;
        //analyseRC();
        //showInformations();
          /*
          for (int index = 0; index < IBUS_MAXCHANNELS; index++)
          {
            Serial.print(rcValueSafe[index]);
            Serial.print(" - ");
          }
          
          Serial.println(";");
          */
        }
      } else {
        //digitalWrite(13, HIGH);  // Checksum error - turn on error LED
      }
      return;
    }
  }
}


void analyseRC()
{
  gazValue = (rcValueSafe[1]-1000)/10;
  directionValue = (rcValueSafe[0]-1000)/10;

  if (oldGlobalModeRcValue != rcValueSafe[8])
  {
    switch (rcValueSafe[8])
    {
      case 1000 : globalMode = 1;break;
      case 1500 : globalMode = 2;break;
      case 2000 : globalMode = 3;break;      
    }  
        
    oldGlobalModeRcValue = rcValueSafe[8];
  }
}

void appliMotorSpeed()
{
  if (oldGazValue != gazValue || oldDirectionValue != directionValue)
  {
    int PWMAvant      = (gazValue * PWMAvEcart / 100) + PWMAvMin;
    int PWMAvantCompl = ((100-gazValue) * PWMArEcart / 100) + PWMArMin;
    
    int PWMDG      = (directionValue * PWMDrEcart / 100) + PWMDrMin;
    int PWMDGCompl = ((100-directionValue) * PWMGaEcart / 100) + PWMGaMin;
    
    analogWrite(5, PWMAvant);
    analogWrite(4, PWMAvantCompl);
    analogWrite(6, PWMDG);
    analogWrite(7, PWMDGCompl);

    oldGazValue = gazValue;
    oldDirectionValue = directionValue;
  }
}

void showInformations ()
{
  //Affichage des infos reçues de la télécommande
  for (int index = 0; index < IBUS_MAXCHANNELS; index++)
  {
    Serial.print(rcValueSafe[index]);
    Serial.print(" - ");
  }

  //Affichage de l'interpretation des ordres qui me concerne
  Serial.print(gazValue);
  Serial.print("% - ");

  Serial.print(directionValue);
  Serial.print("% Mode : ");

  Serial.print(globalMode);
  
  //Fin de ligne
  Serial.println(";");




}