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

//Variables pour le paquet
uint8_t rxbuf[40];
uint8_t packetLen=0;
uint8_t indicePaquet;
uint16_t adresseSource;
uint16_t adresseMulticast;

//Constantes de indices de champs du paquet
#define ADRESSE_MULTICAST 0x7878

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

//Fonction permettant d'extraire l'adresse multicast en gérant 
//L'indexation du paquet
uint16_t extraireMulticast(uint8_t* indice, uint8_t donnees[]){

  uint16_t adresse;
  adresse=wino.decodeUi16(&donnees[*indice]);
  *indice+=2;

  return adresse;
}

void setup() {

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

  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)){

    //Affichage des éléments du paquet reçu
    M5.Lcd.println("=====Reception=====");
    Serial.println("=====Reception=====");
    M5.Lcd.printf("Noeud Source : %X \n", adresseSource);
    Serial.printf("Noeud Source : %X \n", adresseSource);
    adresseMulticast=extraireMulticast(&indicePaquet, rxbuf);
    M5.Lcd.printf("Adresse Multicast : %X \n", adresseMulticast);
    Serial.printf("Adresse Multicast : %X \n", adresseMulticast);
    afficherPayload(indicePaquet, packetLen, rxbuf);

    indicePaquet=0;
  }
}