#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);

uint8_t txbuf[RH_RF95_MAX_MESSAGE_LEN]; //Buffer d'emission 
uint8_t rxbuf[RH_RF95_MAX_MESSAGE_LEN], rxbuflen=sizeof(rxbuf); //Buffer de reception 
uint8_t state, RxSeq, TxSeq, credit;
uint32_t attente;

#define E0 0
#define E1 1
#define E2 2
#define E3 3
#define E4 4
#define E5 5

#define TYPE_DATA 1
#define TYPE_ACK 2 
#define TIMEOUT_ACK 40

#define MAX_CREDIT 5

void setup() {
  M5.begin();
  M5.Power.begin();
  Serial.println("====Emetteur CAP2====");
  M5.Lcd.println("====Emetteur CAP2====");

  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", TxSeq);
      Serial.println();
      M5.Lcd.printf("EMISSION %d \n", TxSeq);
      Serial.println();
      txbuf[0]=TYPE_DATA;
      txbuf[1]=TxSeq;
      txbuf[2]=0xAA;
      txbuf[3]=0x55;

      rf95.send(txbuf,4); //Envoi des données
      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
        Serial.println("CDG expire");
        M5.Lcd.println("CDG expire");
        state=E5;  //Délais expiré
      }
      else{
        if(rf95.recv(rxbuf, &rxbuflen)){
          if((rxbuf[0]==TYPE_ACK) && (rxbuf[1]==TxSeq)){ //Reception d'un ACK avec le bon num de séquence
            state=E4;
          }
          else{
            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=5;
      break;

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

    default:
      state=E0;
      break;
  }

}