#define CLIENT_RANGING_PERIOD 500 //500 //ms. Client performs a ranging cycle every x ms
//#define CLIENT_ADDRESS -1 // all nodes are clients (performs n*(n-1) rangings...)
#define CLIENT_ADDRESS 183 // this node is the client
int delayBetweenAnchors = 20; //50; //ms

#include <SPI.h>
#include <DecaDuino.h>
#include <Ranger.h>
#include <printfToSerial.h>

#ifdef UWB_MODULE_DWM1001 // DecaDev, Yahu, Nomi and others DWM1001 based boards
#include <DWM100x_id.h>
#include <stdio.h>
#else
#include <WiNoIO.h>
#include <EEPROM.h>
#endif

#ifdef __MK20DX256__ // PJRC Teensy based boards aka DecaWiNo
#include <WiNoIO.h>
#include <EEPROM.h>
WiNoIO wino;
DecaDuino decaduino;
#else
DecaDuino decaduino(SS1,DW_IRQ);
#endif

uint32_t rangingNumber;
uint8_t txData[1024];
uint8_t rxData[1024];
uint8_t sqn = 0;
uint16_t rxLen;
uint32_t softwareInterruptDelay;
uint32_t prevSoftwareInterrupt = 0;
uint16_t nodeAddress;

// Expérience de démo classique
//uint16_t anchors[] = {170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 182, 184};

// Noeuds du côté nord du labo (rail 7m)
//uint16_t anchors[] = {100, 101, 102, 103, 104, 105, 106, 107, 109, 110, 111, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 184};
uint16_t anchors[] = {182, 184, 170, 171, 172, 175, 176, 177, 178, 100, 101};
//uint16_t anchors[] = {182, 184, 170, 171, 172, 175, 176, 177, 178};

// Noeuds en GIM
//uint16_t anchors[] = {274, 275, 276, 277, 278, 279, 280, 281};

//uint16_t anchors[] = {115, 185, 186, 187, 188, 189};

//uint16_t anchors[] = {254, 257, 258, 261, 262, 263, 264, 265, 266};
//uint16_t anchors[] = {255, 256, 257, 258, 259, 260, 261};
//uint16_t anchors[] = {119, 115, 186};

// uint16_t anchors[] = {183, 184, 100, 101, 104, 105, 106, 107, 109, 110, 111, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 182, 142, 143, 145, 146, 147, 148, 149, 150, 151, 152, 153};
// Noeuds du côté sud du labo
//uint16_t anchors[] = {142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153};
//uint16_t anchors[] = {115, 123, 188, 156, 259, 267};



int nbAnchors = sizeof(anchors)/sizeof(uint16_t);
int rangingPeriod = -1;
bool messageReceived = false;

Ranger ranger;
Ranger::getRangingRequestStatus_e rangingStatus;
Ranger::ranging_t ranging;
Ranger::message_t txMsg, rxMsg;


void prepareAndSendMessage ( Ranger::message_t *txmsg )
{
  int i, l=0;

  // handle TX power
  // store previous value
  bool previousPowerSettingIsManual = decaduino.isTxPowerManual();
  uint32_t previousPowerSettingValue = decaduino.getTX_POWER();
  // sets requested value
  if (txMsg.manualTxPower){
    decaduino.setManualTxPowerRaw(txMsg.manualTxPowerValue);
  }
  else {
    decaduino.setSmartTxPower();
  }

  txMsg.trxTemperatureBegin = decaduino.getTemperature();
  txMsg.trxVoltageBegin = decaduino.getVoltage();
  decaduino.plmeRxDisableRequest();
  decaduino.encodeUint16(nodeAddress, &txData[l]); l+=2;
  decaduino.encodeUint16(txMsg.destination, &txData[l]); l+=2;
  txData[l++] = ++sqn;
  txData[l++] = txMsg.protocol;
  for (i=0; i<txMsg.payloadLen; i++) txData[l+i] = txMsg.payload[i];
  decaduino.pdDataRequest(txData, l+i);
  while (!decaduino.hasTxSucceeded()) { decaduino.engine(); };
  txMsg.tsTx = decaduino.getLastTxTimestamp();
  txMsg.trxTemperatureEnd = decaduino.getTemperature();
  txMsg.trxVoltageEnd = decaduino.getVoltage();

  // resets TX power to previous value
  if ( previousPowerSettingIsManual ){
    uint8_t value = (previousPowerSettingValue & 0xff00) >> 8; // gets the value of TXPOWPHR register.
    decaduino.setManualTxPowerRaw(txMsg.manualTxPowerValue);
  }
  else {
    decaduino.setSmartTxPower();
  }
}


void processHal()
{
  // This function processes the Hardware Abstraction Layer (HAL) between DecaDuino and Ranger.

  // Call the Decaduino Engine
  decaduino.engine();
  
  // Call the Ranger Engine
  ranger.engine();

  // Is there a message to send?
  if ( ranger.getTxBufferStatus() )
  {
    // A new message has to be sent: disable RX, make packet, send and wait. DOES NOT RE-ENABLE RX.
    int i, l=0;
    prepareAndSendMessage(&txMsg);
    ranger.setTxBufferStatus(Ranger::TX_STATUS_DONE);
  }

  // Enable RX ?
  if ( ranger.getRxRequestStatus() )
  {
    rxMsg.trxTemperatureBegin = decaduino.getTemperature();
    decaduino.plmeRxEnableRequest();
    ranger.rxEnableRequest(false);
  }

  // Does a message has been received?
  if ( decaduino.rxFrameAvailable() )
  {
    // A new message has been received: analyse-it
    uint16_t sourceAddress = decaduino.decodeUint16(&rxData[0]);
    uint16_t destinationAddress = decaduino.decodeUint16(&rxData[2]);
    uint8_t sqn = rxData[4];
    uint8_t msgType = rxData[5];
    if ( (destinationAddress == nodeAddress) || (destinationAddress == 0xffff) )
    {
      // This message is for me or broadcast
      rxMsg.source = sourceAddress;
      rxMsg.destination = destinationAddress;
      rxMsg.tsRx = decaduino.getLastRxTimestamp();
      rxMsg.skew = decaduino.getLastRxSkew();
      rxMsg.nlosIndication = decaduino.getNLOSIndication();
      rxMsg.protocol = (Ranger::ranger_protocols_e)msgType;
      rxMsg.RSSI = decaduino.getRSSI();
      rxMsg.FP_POWER = decaduino.getFpPower();
      rxMsg.PA_POWER = decaduino.getPeakPower();
      for (int i=0; i<rxLen; i++) rxMsg.payload[i] = rxData[6+i];
      rxMsg.trxTemperatureEnd = decaduino.getTemperature();
      if ( rxMsg.protocol == Ranger::DATA_PROTOCOL )
      {
        messageReceived = true;
        decaduino.plmeRxEnableRequest();
      }
      else ranger.newRxMessage();
    }
    else
    {
      // This message is not for me. Re-enable RX
      decaduino.plmeRxEnableRequest();
    }
  }
}


void initBoard(void)
{
  #ifdef __MK20DX256__ // PJRC Teensy based boards aka DecaWiNo
  if (!wino.loadConfig() ) while (1);
  SPI.setSCK(wino.getTrxSckPin());
  wino.rgbDraw(0,0,0);
  nodeAddress = wino.getLabel();
  #endif
  
  #ifdef ARDUINO_DWM1001_DEV // DecaDev board
  pinMode(LED_RED_BOT,OUTPUT);
  pinMode(LED_RED_TOP,OUTPUT);
  pinMode(LED_BLUE,OUTPUT);
  pinMode(LED_GREEN,OUTPUT);
  digitalWrite(LED_GREEN,HIGH);   // for ARDUINO_DWM1001_DEV, HIGH == LED off
  digitalWrite(LED_BLUE,HIGH);    // for ARDUINO_DWM1001_DEV, HIGH == LED off
  digitalWrite(LED_RED_TOP,HIGH); // for ARDUINO_DWM1001_DEV, HIGH == LED off
  digitalWrite(LED_RED_BOT,HIGH); // for ARDUINO_DWM1001_DEV, HIGH == LED off
  nodeAddress = toNodeID(getDwMacAddrUint64());
  #endif

  #ifdef ARDUINO_YAHU
  pinMode(LED_RED,OUTPUT);
  pinMode(LED_GREEN,OUTPUT);
  pinMode(LED_BLUE,OUTPUT);
  nodeAddress = toNodeID(getDwMacAddrUint64());
  #endif
}


void rgbDraw(int r, int g, int b)
{
  #ifdef __MK20DX256__ // PJRC Teensy based boards aka DecaWiNo
  wino.rgbDraw(r,g,b);
  #endif
  
  #ifdef ARDUINO_DWM1001_DEV // DecaDev board
  if (r<16) digitalWrite(LED_RED_TOP, LOW); else digitalWrite(LED_RED_TOP, HIGH);
  if (g<16) digitalWrite(LED_GREEN, LOW); else digitalWrite(LED_GREEN, HIGH);
  if (b<16) digitalWrite(LED_BLUE, LOW); else digitalWrite(LED_BLUE, HIGH);
  #endif

  #ifdef ARDUINO_YAHU // Yahu and Nomi boards
  analogWrite(LED_RED, r);
  analogWrite(LED_GREEN, g);
  analogWrite(LED_BLUE, b);
  #endif  
}


void setup()
{
  Serial.begin(115200);
  while ( !decaduino.init())
  {
    Serial.println("DecaDuino init failed... retry...");
    #ifndef ARDUINO_DWM1001_DEV
    wino.rgbDraw(255,0,0); // Red
    #else
    digitalWrite(LED_RED_BOT, HIGH);delay(100);digitalWrite(LED_RED_BOT, LOW);
    #endif
    delay(100);
  }

  if ( !ranger.init() )
  {
    Serial.println("Ranger init failed. Freeze.");
    while(1) {
      #ifndef ARDUINO_DWM1001_DEV
      wino.rgbDraw(255,0,0); delay(50); wino.rgbDraw(0,0,0); delay(50);
      #else
      digitalWrite(LED_RED_TOP,HIGH); digitalWrite(LED_RED_BOT,LOW); delay(50); digitalWrite(LED_RED_TOP,LOW); digitalWrite(LED_RED_BOT,HIGH); delay(50);
      #endif
      }
  }


  #ifndef ARDUINO_DWM1001_DEV
  nodeAddress = wino.getLabel();
  #else
  nodeAddress = toNodeID(getDwMacAddrUint64());
  #endif

  printf("My nodeAddress is %d\n", nodeAddress);

  // Configure DecaDuino
  decaduino.setRxBuffer(rxData, &rxLen);


/*
  // Best PHY config for NLOS and long range
  decaduino.setChannel(4);
  decaduino.setPrf(64);
  decaduino.setPcode(17);
  decaduino.setDataRate(DW1000_DATARATE_850KBPS);
  decaduino.setDecaWaveSFD();
  decaduino.setPreambleLength(4096);
  decaduino.enableNLOSTunings();
*/

  // Configure Ranger
  ranger.setDebug(false);
  ranger.setTxBuffer(&txMsg);
  ranger.setRxBuffer(&rxMsg);
  ranger.setNodeAddress(nodeAddress);
  ranger.enableTwrServer(true);
  rgbDraw(255,0,255);

  decaduino.setManualTxPower(COARSE_POWER_SETTING::COARSE_POWER_GAIN_0db,0);
  // Various
  if ( ( nodeAddress == CLIENT_ADDRESS ) || ( CLIENT_ADDRESS == -1) )
  {
    rangingPeriod = CLIENT_RANGING_PERIOD;
  }
  softwareInterruptDelay = rangingPeriod;
  prevSoftwareInterrupt = millis();
  rangingNumber = 0;
}


void loop()
{
  processHal(); // Call the HAL for incoming/outcoming messages

  // Periodic action (non blocking instructions)
  if ( (millis() - prevSoftwareInterrupt) > softwareInterruptDelay)
  {
    prevSoftwareInterrupt = millis();
    
    // This is the client node
    if ( rangingPeriod != -1 )
    {
      if ( rangingNumber%nbAnchors == nbAnchors-1 )
      {
        // End of a ranging cycle: plan next cycle and sleep transceiver
        softwareInterruptDelay = rangingPeriod;
      }
      else
      {
        // Next anchor
        softwareInterruptDelay = delayBetweenAnchors;
      }
      rgbDraw(0,0,255);
      ranging.distanceOfOneTofStep = RANGING_UNIT;
      ranging.other = anchors[rangingNumber%nbAnchors];
      ranging.protocol = Ranger::TWR_PROTOCOL;
      ranger.rangingRequest(&ranging);
    }
  }

  // Returns after a ranging process by Ranger
  if ( ranger.rangingIndication(&rangingStatus) )
  {
    Serial.flush();
    switch ( rangingStatus )
    {
      case Ranger::RANGING_STATUS_ERROR:
        printf("Error while ranging with %d\n", ranging.other);
        rgbDraw(255,0,0);
        break;

      case Ranger::RANGING_STATUS_DONE:
        //printf("Result for this ranging with %04x:\n", ranging.other);
        //ranger.printRanging(&ranging);
        ranger.printRangingShortAsJson(&ranging);
        printf("\r\n");
        //ranger.printRangingShortAsJson(&ranging); printf("\n"); // printf("\n") is required to flush the data.
        rgbDraw(0,0,0);
        break;
      default:
        break;
    }
    Serial.flush();
    rangingNumber++;
    #ifndef ARDUINO_DWM1001_DEV
    wino.rgbDraw(0,0,0);
    #else
    digitalWrite(LED_BLUE,HIGH);
    digitalWrite(LED_GREEN,HIGH);
    digitalWrite(LED_RED_TOP,HIGH);
    digitalWrite(LED_RED_BOT,HIGH);
    #endif
  }

  // If a DATA message has been received
  if ( messageReceived )
  {
    uint16_t from = rxMsg.source;

    // Do what you want with this message
    // ...

    messageReceived = false; // and clean the flag
  }
}