#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 0x0002
#define CANAL 7

//Variables pour le paquet
uint8_t rxbuf[40]; //Buffer de reception
uint8_t packetLen; //Longueur du paquet reçu
uint16_t adresseSource; //Adresse du noeud source
uint16_t adresseMulticast; //Adresse multicast reçue

//Propriétés du paquet
#define ADRESSE_MULTICAST 0x7878
#define TTL_MAX 2 //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


//Procédure d'affichage de la payload reçue
//À partir de l'indice de début de payload 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("|");
}


void setup() {

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

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

void loop() {
  wino.process();

  //Test Reception
  if(wino.recv(&adresseSource, rxbuf, &packetLen) /*&& (adresseSource!=1)*/){
    //Affichage des éléments du paquet reçu
    M5.Lcd.println("=====Reception=====");
    Serial.println("=====Reception=====");
    //Affichage du noeud source
    adresseSource=wino.decodeUi16(&rxbuf[CHAMP_ADRESSE_SRC]);
    M5.Lcd.printf("Noeud Source : %X \n", adresseSource);
    Serial.printf("Noeud Source : %X \n", adresseSource);
    //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, packetLen, rxbuf);

    //Test pour la réemission
    //Paquet non émis par le noeud
    if(adresseSource != ADRESSE_NOEUD){
      //Même adresse multicast et TTL inférieur au max
      if((adresseMulticast==ADRESSE_MULTICAST) && (rxbuf[CHAMP_TTL]<=TTL_MAX)){
        M5.Lcd.println("=====Réemission Paquet=====");
        M5.Lcd.println("=====Réemission Paquet=====");
        wino.send(0xFFFF, rxbuf, packetLen); 
      }
    }
    else{
      M5.Lcd.println("=== Paquet emis par le noeud ===");
      Serial.println("=== Paquet emis par le noeud ===");
    }
    
  }
}