#include <Arduino.h>
#include <SPI.h>
#include "SimpleWiNo.h"
#include <M5Stack.h>

//Objet Wino
SimpleWiNo wino;

//Constantes de configuration des noeuds
#define ADRESSE_CLUSTER 0x4A89
#define ADRESSE_NOEUD 0x0001
#define CANAL 7
#define DELAI_ENVOI 60000

//Variables pour le paquet
uint8_t txbuf[40]; //Buffer d'émission
uint8_t rxbuf[40]; //Buffer de réception
uint8_t packetLenTX; //Longueur du paquet émis
uint8_t packetLenRX; //Longueur du paquet reçu
uint16_t adresseMacSource; //Adresse de couche 2 du noeud source
uint16_t adresseIpSource; //Adresse de couche 3 du noeud source
uint16_t adresseDestination; //Adresse du noeud source
uint16_t adresseMulticast; //Adresse multicast reçue
uint8_t numSeq=0; //Numéro de séquence incrémenté pour l'envoi

//Propriétés du paquet 
#define ADRESSE_MULTICAST 0x7878 //Adresse multicast du noeud
#define NB_PAYLOAD 10 //Nombre de données de payload
#define TTL_MAX 7 //Time To Live avant expiration

//Indices des champs du paquet
#define CHAMP_ADRESSE_DST 0 //Position de l'adresse de destination
#define CHAMP_TTL 2 //Position du TTL dans le paquet
#define CHAMP_ADRESSE_SRC 3 //Position du champ d'adressage
#define CHAMP_NUM_SEQ 5 //Position du numéro de séquence
#define CHAMP_MIN_PAYLOAD 6 //Position du premier octet de la payload

//Mémorisation des paquets

//Nombre de paquets maximums mémorisables
#define NB_PAQUET_MEM 10

//Structure pour l'identification
struct identifiant{
  uint16_t srcAddr;
  uint8_t sqn;
};
typedef struct identifiant IDENTIFIANT;

uint8_t indiceMemId=0; //Indice pour la mise à jour dans le tableau
IDENTIFIANT idRecu; //Identifiant courant du paquet reçu
IDENTIFIANT tabId[NB_PAQUET_MEM]; //Tableau d'identifiants

//Procédures pour la mémorisation

//Initialisation du tableau d'identifiants
void initTabId(IDENTIFIANT tab[]){
  for(int i=0; i<NB_PAQUET_MEM; i++){
    tab[i].srcAddr=0xFFFF;
    tab[i].sqn=255;
  }
}

//Affichage des id du tableau
void AfficheId(IDENTIFIANT tab[]){

  for(int i=0; i<NB_PAQUET_MEM; i++){
    M5.Lcd.printf("{%02X, %02X}", tab[i].srcAddr, tab[i].sqn);
    Serial.printf("{%02X, %02X}", tab[i].srcAddr, tab[i].sqn);
  }
  M5.Lcd.println();
  Serial.println();
}

//Ajout d'un nouvel identifiant dans le tableau
//L'indice du tableau suit un modulo pour reboucler dans les mises à jour
//Afin de ne garder que les derniers NB_PAQUET_MEM paquets
void ajoutTabId(IDENTIFIANT id, IDENTIFIANT tab[], uint8_t* indice){
  tab[*indice]=id;
  *indice=(*indice+1)%NB_PAQUET_MEM;
}

//Fonction vérifiant si l'id du paquet arrivé a déjà été routé
//Si c'est le cas la fonction renvoi 1 (VRAI)
//Sinon, elle renvoi 0 (FAUX)
int PaquetDejaRoute(IDENTIFIANT id, IDENTIFIANT tab[]){

  for(int i=0; i<NB_PAQUET_MEM; i++){
    //Un élément du tableau correspond
    if((tab[i].srcAddr==id.srcAddr) && (tab[i].sqn==id.sqn)){
      return 1;
    }
  }
  return 0;
}

//Procédures pour l'envoi

//Procédure de formatage de l'en tête et de la payload
//du paquet en émission
void formaterPaquet(uint8_t* longueur, uint8_t donnees[], uint8_t nSeq){

  *longueur=0;

  //Ajout En-tete

  //Adresse Destination
  wino.encodeUi16(ADRESSE_MULTICAST, &donnees[CHAMP_ADRESSE_DST]);
  *longueur +=2;

  //Ajout du TTL
  donnees[CHAMP_TTL]=0;
  *longueur+=1;

  //Adresse Source
  wino.encodeUi16(ADRESSE_NOEUD, &donnees[CHAMP_ADRESSE_SRC]);
  *longueur +=2;

  //Numéro de séquence
  donnees[CHAMP_NUM_SEQ]=nSeq;
  *longueur +=1;

  //Ajout de la payload
  for(int i=0; i<NB_PAYLOAD; i++){
    donnees[*longueur]=random(0,256);
    *longueur+=1;
  }
}

//Affichage du paquet à émettre
void afficherPaquet(uint8_t longueur, uint8_t donnees[]){
  
  for(int i=0; i<longueur; i++){
    M5.Lcd.printf("|%02X", donnees[i]);
    Serial.printf("|%02X", donnees[i]);
  }
  M5.Lcd.println("|");
  Serial.println("|");
}

//Procédures pour la réception

//Procédure d'affichage de la payload reçue
//À partir de l'indice de début de la uniquement
void afficherPayload(uint8_t indiceDebut, uint8_t longueur, uint8_t donnees[]){
  
  M5.Lcd.printf("Payload : ");
  Serial.printf("Payload : ");
  for(int i=indiceDebut; i<longueur; i++){
    M5.Lcd.printf("|%02X", donnees[i]);
    Serial.printf("|%02X", donnees[i]);
  }
  M5.Lcd.println("|");
  Serial.println("|");
}

//Condition pour déclencher un envoi toute les 20 secondes
uint32_t attente = millis()+DELAI_ENVOI;

void setup() {

  //Démarage M5
  M5.begin();
  M5.Power.begin();
  Serial.println("====Emetteur sprint 5====");
  M5.Lcd.println("====Emetteur sprint 5====");

  delay(3000);

  //Demarage wino
  wino.init();
  //Adresse cluster
  wino.set(NODE_PANID, ADRESSE_CLUSTER);
  //Adresse noeud
  wino.set(NODE_SHORT_ADDRESS, ADRESSE_NOEUD);
  //Fréquence de travail
  wino.set(NODE_CHANNEL, CANAL);

  //Générateur de payload aléatoire 
  randomSeed(analogRead(30));

  //Initialisation du tableau de mémoire des identifiants
  initTabId(tabId);
}

void loop() {
  wino.process();

  //Envoi d'un paquet toutes les DELAI_ENVOI secondes
  if(millis() > attente){
    M5.Lcd.println("====Envoi Paquet====");
    Serial.println("====Envoi Paquet====");
    formaterPaquet(&packetLenTX, txbuf, numSeq);
    afficherPaquet(packetLenTX, txbuf);
    wino.send(0xFFFF, txbuf, packetLenTX);
    numSeq+=1;
    attente=millis()+DELAI_ENVOI;
  }
  
  //Test Reception
  if(wino.recv(&adresseMacSource, rxbuf, &packetLenRX) && (adresseMacSource!=3)){
    //Affichage des éléments du paquet reçu
    M5.Lcd.println("=====Reception=====");
    Serial.println("=====Reception=====");
    //Affichage du noeud source
    adresseIpSource=wino.decodeUi16(&rxbuf[CHAMP_ADRESSE_SRC]);
    M5.Lcd.printf("MAC SRC : %X | ", adresseMacSource);
    Serial.printf("MAC SRC : %X | ", adresseMacSource);
    M5.Lcd.printf("IP SRC : %X \n", adresseIpSource);
    Serial.printf("IP SRC : %X \n", adresseIpSource);
    //Affichage Adresse Multicast
    adresseMulticast=wino.decodeUi16(&rxbuf[CHAMP_ADRESSE_DST]);
    M5.Lcd.printf("Adresse Multicast : %X \n", adresseMulticast);
    Serial.printf("Adresse Multicast : %X \n", adresseMulticast);
    //Affichage TTL
    M5.Lcd.printf("TTL : %d \n", rxbuf[CHAMP_TTL]);
    Serial.printf("TTL : %d \n", rxbuf[CHAMP_TTL]);
    //Incrémentation TTL
    rxbuf[CHAMP_TTL]+=1;
    //Affichage du numéro de séquence
    M5.Lcd.printf("Num Seq : %d \n", rxbuf[CHAMP_NUM_SEQ]);
    Serial.printf("Num Seq : %d \n", rxbuf[CHAMP_NUM_SEQ]);
    //Affichage Payload
    afficherPayload(CHAMP_MIN_PAYLOAD, packetLenRX, rxbuf);

    //Test pour la réemission
    //Le paquet n'a pas été émis par le noeud lui-même
    if(adresseIpSource!=ADRESSE_NOEUD){
      //Même adresse multicast et TTL inférieur au max
      if((adresseMulticast==ADRESSE_MULTICAST) && (rxbuf[CHAMP_TTL]<=TTL_MAX)){

        //Récupération de l'identifiant du paquet recu
        idRecu.srcAddr=adresseIpSource;
        idRecu.sqn=rxbuf[CHAMP_NUM_SEQ];

        //Test si le paquet n'a pas déjà été reçu
        if(!PaquetDejaRoute(idRecu, tabId)){
          M5.Lcd.println("=====Reemission Paquet=====");
          Serial.println("=====Reemission Paquet=====");
          //Ajout du nouvel id dans le tableau
          ajoutTabId(idRecu, tabId, &indiceMemId);
          //Réemission du paquet
          wino.send(0xFFFF, rxbuf, packetLenRX); 

          //Affichage des id mémorisés 
          AfficheId(tabId);
        }
        else{
          M5.Lcd.println("===Deja Recu===");
          Serial.println("===Deja Recu===");
        }
      }
      else{
        M5.Lcd.println("Adresse multicast incorecte ou TTL depasse");
        Serial.println("Adresse multicast incorecte ou TTL depasse");
      }
    }
    else{
      M5.Lcd.println("===Paquet emis par le noeud===");
      Serial.println("===Paquet emis par le noeud===");
    }
    
  }
}