#include <SPI.h>
#include <RH_RF95.h>
#include <M5Stack.h>

//Déclaration des constantes
#define RFM95_CS 5 //M5 LoRa
#define RFM95_DIO0 36 //M5 LoRa
RH_RF95 rf95(RFM95_CS, RFM95_DIO0);

//Variables 
uint8_t txbuf[RH_RF95_MAX_MESSAGE_LEN]; 
uint8_t rxbuf[RH_RF95_MAX_MESSAGE_LEN], rxbuflen=sizeof(rxbuf); //Buffer de reception et longueur
uint8_t state, TxSeq, credit;
uint32_t attente;
uint8_t FCS; //Champ de contrôle

//Constantes d'états
#define E0 0
#define E1 1
#define E2 2
#define E3 3
#define E4 4
#define E5 5

//Constantes de données et d'acquitement
#define TYPE_DATA 1
#define TYPE_ACK 2 
#define TIMEOUT_ACK 40
#define MAX_CREDIT 5

//Constantes de champs de trame
#define CHAMP_TYPE 0
#define CHAMP_NUM_SEQ 1
#define CHAMP_MIN_PAYLOAD 2
#define CHAMP_MAX_PAYLOAD 19
#define CHAMP_FCS_DATA 20
#define CHAMP_FCS_ACK 2


int erreur_ack=1;


void setup() {
  M5.begin();
  M5.Power.begin();
  Serial.println("===Emmetteur CAP3 DET===");
  M5.Lcd.println("===Emmetteur CAP3 DET===");

  delay(3000);

  if(!rf95.init()){
    Serial.println("Echec de la mise en place");
    M5.Lcd.println("Echec de la mise en place");
  }
  else{
    Serial.println("Mise en place reussie !!!");
    M5.Lcd.println("Mise en place reussie !!!");
    //Configuration de la radio
    rf95.setModemConfig(RH_RF95::Bw125Cr45Sf128);
    rf95.setFrequency(867.8);
    state=E0;

    TxSeq=0;
    credit=MAX_CREDIT;
    Serial.println("Boucle principale.");
    M5.Lcd.println("Boucle principale.");
    delay(3000);
  }

}

void loop() {
  
  switch(state){
    case E0: //Code source à l'état d'émission
      Serial.printf("EMISSION %d\n\n", TxSeq);
      M5.Lcd.printf("EMISSION %d\n\n", TxSeq);
      txbuf[CHAMP_TYPE]=TYPE_DATA;
      txbuf[CHAMP_NUM_SEQ]=TxSeq;

      //On remplit les 18 octets de payload au max (255)
      for (int i=CHAMP_MIN_PAYLOAD;i<=CHAMP_MAX_PAYLOAD;i++){  
        txbuf[i]=255;
      }

      //Calcul du FCS : XOR
      FCS=0; //Champ sur un octet
      for(int i=0;i<=CHAMP_MAX_PAYLOAD;i++){
        //XOR des 20 octets de la trame.
        FCS=FCS ^ txbuf[i];
      }
      //On ajoute le FCS calculé à la fin de la trame
      txbuf[CHAMP_FCS_DATA] = FCS;

      //Affichage pour le debug 
      for(int i=0; i<21; i++){
        Serial.printf("|%02X", txbuf[i]);
        M5.Lcd.printf("|%02X", txbuf[i]);
      }
      Serial.println("|");
      M5.Lcd.println("|");

      //Envoi des données
      rf95.send(txbuf,21); 
      rf95.waitPacketSent();

      credit-=1;
      state=E1;
      break;

    case E1:
      attente = millis()+TIMEOUT_ACK; //Armement du Chien de Garde
      state = E2;
      break;

    case E2:
      rf95.setModeRx();
      state=E3;
      break;
    
    case E3:
      if(millis() > attente){  //Test expiration CdG
        state=E5;  //Délais expiré
      }
      else{
        if(rf95.recv(rxbuf, &rxbuflen)){
          //Simulation d'erreur

          if(erreur_ack%3==0){
            rxbuf[CHAMP_FCS_ACK]=rxbuf[CHAMP_FCS_ACK]+1;
          }
          erreur_ack+=1;
           
          //Reception d'un ACK avec le bon num de séquence 
          if((rxbuf[CHAMP_TYPE]==TYPE_ACK) && (rxbuf[CHAMP_NUM_SEQ]==TxSeq)){ 
            //Trame reçue sans erreur
            if((rxbuf[CHAMP_TYPE] ^ rxbuf[CHAMP_NUM_SEQ])==rxbuf[CHAMP_FCS_ACK]){
              //Affichage de la trame reçue
              state=E4;
            }
            else{
              //Affichage de l'échec
              state=E5;
            }
          }
          else{
            //Réécoute
            state=E2;
          }
        }
      }
      break;

    case E4:
      Serial.printf("ACK_RECU [%d] \n", TxSeq);
      M5.Lcd.printf("ACK_RECU [%d] \n", TxSeq);
      state=E0, TxSeq+=1, credit=MAX_CREDIT;
      break;

    case E5:
      Serial.print("E5   ");
      M5.Lcd.print("E5   ");
      if (credit==0){
        Serial.print("ECHEC\n");
        M5.Lcd.print("ECHEC\n");
        credit=MAX_CREDIT;
        TxSeq+=1;
      }
      else{
        Serial.printf("Nouvelle tentative n %d\n", MAX_CREDIT-credit);
        M5.Lcd.printf("Nouvelle tentative n %d\n", MAX_CREDIT-credit);
        Serial.println();
        M5.Lcd.println();
      }
      state=E0;
      break;

    default:
      state=E0;
      break;
  }
}