#include <Arduino.h>
#include <LowPower.h>
#include <SPI.h>
#include "definition.h"
#include "BoardConfig.h"
#include "MBPeripherals.h"

BoardConfig defaultBC;
MBPeripherals MBP(defaultBC);

MailboxDataPacket payload;
uint8_t rCode(0);

void setup()
{
  #if SERIAL_DEBUG_ENABLED 
    Serial.begin(SERIAL_BAUD_RATE);
    Serial.println("Setup begin");
  #endif
  
  rCode = MBP.init();
  memset(&payload, 0, sizeof payload);

  #if SERIAL_DEBUG_ENABLED
  Serial.print("Payload size : ");Serial.println(sizeof payload);
  debugStruct(&payload);
  #endif
  payload.header = HEADER_e::CONNECTED_MAILBOX;

  #if SERIAL_DEBUG_ENABLED
  Serial.print("WSP init returned : ");Serial.println(rCode);
  Serial.println("Setup end");
  //Let time to the uart to spit out what's left
  delay(100);
  #endif

  LowPower.powerDown(SLEEP_FOREVER, ADC_OFF, BOD_OFF);
}

void loop()
{

  MBP.maskExtInt();
  if((payload.mailbox_event = MBP.readMailboxEvent()) != MAILBOX_EVENT_e::MAILBOX_UNKNOWN)
  {
    payload.battery = MBP.batteryVoltage();

    //We now send the event
    MBP.externalPeripherals(MBPeripherals::State::ON);
    rCode = MBP.initExternalPeripherals();

    #if SERIAL_DEBUG_ENABLED
    Serial.print("MBP Ext Peripheral : "); Serial.println(rCode);
    debugStruct(&payload);
    #endif

    if(MBP.getRadio().isChipConnected())
    {
      MBP.applyRadioConfig();
      MBP.getRadio().setRetries(15, 15);
      MBP.getRadio().openWritingPipe((const uint8_t *)RADIO_NODE_ADDRESS);
      MBP.getRadio().stopListening(); //Very important, if not called, no ack will be received !

      for(uint8_t i(0); i < MAX_SEND_ROUND_RETRIES; i++)
      {
        if(MBP.getRadio().write(&payload, sizeof payload))
        {
          #if SERIAL_DEBUG_ENABLED
          Serial.println("Payload sent !");
          delay(100);
          #endif
          break;
        }
        #if SERIAL_DEBUG_ENABLED
        else
        {
          Serial.println("Failed to send payload !");
          delay(100);
        }
        #endif
      }
    }
    #if SERIAL_DEBUG_ENABLED
    else
    {
      Serial.println("NRF missing !");
      delay(100);
    }
    #endif

    MBP.externalPeripherals(MBPeripherals::State::OFF);
    
    //We wait for some time before allowing next trigger
    //Done to prevent switch bounces
    LowPower.powerDown(SLEEP_2S, ADC_OFF, BOD_OFF);
  }
  #if SERIAL_DEBUG_ENABLED
  else
  {
    Serial.println("Unknown event :O !");
    delay(100);
  }
  #endif
  
  MBP.unmaskExtInt();
  payload.id++;
  //Sleep until next trigger happens
  LowPower.powerDown(SLEEP_FOREVER, ADC_OFF, BOD_OFF);
}

void debugStruct(MailboxDataPacket *p)
{
  Serial.println("##############DATA##############");
  Serial.print("ID : ");
  Serial.println(p->id);

  Serial.print("HEADER : ");
  Serial.println(p->header);
  
  Serial.print("BATT : ");
  Serial.print(p->battery);
  Serial.println(" V");
  
  Serial.print("EVENT : ");
  switch(p->mailbox_event)
  {
    case MAILBOX_LETTER:
      Serial.println("LETTER");
      break;
    case MAILBOX_PACKAGE:
      Serial.println("PACKAGE");
      break;
    case MAILBOX_COLLECTED:
      Serial.println("COLLECTED");
      break;
    default:
      Serial.println("UNKNOWN");
      break;
  }

  delay(100); //Needed to prevent the micro to go to sleep to quickly and thus not sending the full serial output !
}