#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 state;
uint8_t txbuf[RH_RF95_MAX_MESSAGE_LEN];
uint8_t rxbuf[RH_RF95_MAX_MESSAGE_LEN], rxbuflen=sizeof(rxbuf);
uint8_t RxSeq;
uint8_t FCSc, FCSr; //Champs de contrôle d'un octet calculé et reçu

//Constantes d'état
#define E0 0
#define E1 1
#define E2 2
#define E3 3

#define TYPE_DATA 1
#define TYPE_ACK 2

//Constantes de champs de trames
#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

void setup() {
  M5.begin();
  M5.Power.begin();
  Serial.println("===Recepteur CAP3 DET===");
  M5.Lcd.println("===Recepteur 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 !!!");
    rf95.setModemConfig(RH_RF95::Bw125Cr45Sf128);
    rf95.setFrequency(867.8);

    state=E0;
    Serial.println("Boucle principale");
    M5.Lcd.println("Boucle principale");
    RxSeq=255;
    delay(1000);
  }

}

void loop() {
  switch(state){

    case E0: //ECOUTE
      Serial.println("E0 : Attente Reception \n");
      M5.Lcd.println("E0 : Attente Reception \n");
      rf95.setModeRx();
      state=E1;
      break;

    case E1: 
      if(rf95.recv(rxbuf, &rxbuflen)){
        if(rxbuf[CHAMP_TYPE]==TYPE_DATA){
          //Test si FCS juste
          FCSr=rxbuf[CHAMP_FCS_DATA]; //Champ de contrôle reçu
          //Calcul du FCS
          FCSc=0;
          for(int i=0;i<=CHAMP_MAX_PAYLOAD;i++){
            FCSc= FCSc ^ rxbuf[i];
          }
          //Mêmes FCS : Trame juste
          if(FCSr==FCSc){
            state=E2;
          }
          //Sinon trame fausse
          else{
            Serial.println();
            M5.Lcd.println();
            Serial.printf("Erreur de Trame, FCSr=%d FCSc=%d", FCSr, FCSc);
            M5.Lcd.printf("Erreur de Trame, FCSr=%d FCSc=%d", FCSr, FCSc);
            Serial.println();
            M5.Lcd.println();
            state=E0;
          }
        }
        else{
          state=E0;
        }
      }
      break;
    
    case E2:

      if(RxSeq!=rxbuf[CHAMP_NUM_SEQ]){
        RxSeq=rxbuf[CHAMP_NUM_SEQ];
        Serial.printf("Num Seq : [%d] \n", RxSeq);
        M5.Lcd.printf("Num Seq : [%d] \n", RxSeq);
        Serial.printf("DATA de %d octets :", rxbuflen);
        M5.Lcd.printf("DATA de %d octets :", rxbuflen);
        Serial.println();
        M5.Lcd.println();
        for(int j=0;j<rxbuflen;j++){
          Serial.printf("%02x|", rxbuf[j]);
          M5.Lcd.printf("%02x|", rxbuf[j]);
        }
        Serial.println();
        M5.Lcd.println();
      }
      else{
        Serial.printf("Duplication\n");
        M5.Lcd.printf("Duplication\n");
      }
      state=E3;
      break;

    case E3:
      Serial.println("E3 : Envoi ACK");
      M5.Lcd.println("E3 : Envoi ACK");
      txbuf[CHAMP_TYPE]=TYPE_ACK;
      txbuf[CHAMP_NUM_SEQ]=rxbuf[CHAMP_NUM_SEQ];
      //Ajout du FCS du ACK
      txbuf[CHAMP_FCS_ACK]=txbuf[CHAMP_TYPE] ^ txbuf[CHAMP_NUM_SEQ];
      rf95.send(txbuf,3);
      rf95.waitPacketSent();
      state=E0;
      break;
    
    default:
      break;
  }
}