#include "definition.h"
#include "BoardConfig.h"
#include "WSPeripherals.h"
#include <LowPower.h>

BoardConfig defaultBC;
WSPeripherals WSP(defaultBC);

uint8_t sleepSlots(0), rCode(0);
DataPacket payload;

void setup()
{
  #if SERIAL_DEBUG_ENABLED == 1
  Serial.begin(SERIAL_BAUD_RATE);
  Serial.println("Setup begin");
  #endif

  rCode = WSP.init();
  memset(&payload, 0, sizeof payload);
  #if SERIAL_DEBUG_ENABLED == 1
  Serial.print("Payload size : ");Serial.println(sizeof payload);
  debugStruct(&payload);
  #endif
  payload.header = WEATHER_STATION;
  
  #if SERIAL_DEBUG_ENABLED == 1
  Serial.print("WSP init returned : ");Serial.println(rCode);
  Serial.println("Setup end");
  #endif
}

void loop()
{
  if(sleepSlots == SLEEP_4_SEC_INTERVAL || !sleepSlots)
  {
    //We get all the measurements.
    WSP.externalPeripherals(WSPeripherals::ON);
    rCode = WSP.initExternalPeripherals();

    #if SERIAL_DEBUG_ENABLED == 1
    Serial.print("WSP peripheral init returned : ");Serial.println(rCode);
    #endif

    payload.battery = WSP.batteryVoltage();
    payload.ldr = WSP.sunlightMeasurement();
    WSP.temperatureAndATMPressureFromBMP280(&payload.bmpTemp, &payload.bmpPress);
    payload.humidity = WSP.humidity();
    payload.compensatedHumidity = WSP.compensatedHumidity();
    payload.htuTemp = WSP.temperatureFromHTU21();

    #if SERIAL_DEBUG_ENABLED == 1
    debugStruct(&payload);
    #endif

    //We send it over the air.
    if(WSP.getRadio().isChipConnected())
    {
      WSP.applyRadioConfig();
      //WSP.getRadio().setPayloadSize(sizeof payload); //Does not work
      WSP.getRadio().setRetries(10,20);
      WSP.getRadio().openWritingPipe((const uint8_t *)RADIO_NODE_ADDRESS);
      bool result = WSP.getRadio().write(&payload, sizeof payload);
      #if SERIAL_DEBUG_ENABLED == 1
      if(result)
        Serial.println("Payload sent !");
      else
        Serial.println("Failed to send payload !");
      delay(100);
      #endif
    }
    #if SERIAL_DEBUG_ENABLED == 1
    else
    {
      Serial.println("NRF missing !");
      delay(100);
    }
    #endif
    
    WSP.externalPeripherals(WSPeripherals::OFF);
    payload.id++;
    sleepSlots = 0;
  }

  //Lets sleep for 4 secondes
  LowPower.powerDown(SLEEP_4S, ADC_OFF, BOD_OFF);
  sleepSlots++;
}

void debugStruct(DataPacket *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("LDR : ");
  Serial.print(p->ldr);
  Serial.println("/1023");

  Serial.print("BMP TEMP : ");
  Serial.print(p->bmpTemp);
  Serial.println(" *C");
  
  Serial.print("BMP PRESS : ");
  Serial.print(p->bmpPress);
  Serial.println(" Pa");

  Serial.print("HUM : ");
  Serial.print(p->humidity);
  Serial.println(" %");
  
  Serial.print("COM HUM : ");
  Serial.print(p->compensatedHumidity);
  Serial.println(" %");
  
  Serial.print("HTU TEMP : ");
  Serial.print(p->htuTemp);
  Serial.println(" *C");
}