#include <Arduino.h>
#include <RHReliableDatagram.h>
#include <RH_NRF24.h>
#include <SPI.h>
#include "radio.h"

static unsigned long lastSerialTime;

RcData data;

RcData* getDataptr( void )
{
  return &data;
}

void radio_c::begin(  )
{
  if (!RadioManager.init())
  {
    #ifdef DEBUG
    Serial.println("init failed");
    #endif
    open = false;
  }
  else
  {
    open = true;
  }
}

bool radio_c::transmitData( void ) 
{
  RcData* data = getDataptr();
  uint8_t* payload = data->getByteArray();
  if (RadioManager.sendtoWait(payload, sizeof(payload), m_myaddress))
  {
    // Now wait for a reply from the server
    uint8_t len = sizeof(buf);
    uint8_t from;
    if (RadioManager.recvfromAckTimeout(buf, &len, 2000, &from))
    {
      #ifdef DEBUG
        Serial.print("got reply from : 0x");
        Serial.print(from, HEX);
        Serial.print(": ");
        Serial.println((char*)buf);
      #endif
      return true;
    }
    else
    {
      #ifdef DEBUG
        Serial.println("No reply, is nrf24_reliable_datagram_server running?");
      #endif
      return false;
    }
  }
  else
  {
    #ifdef DEBUG
      Serial.println("sending failed");
    #endif
    return false;
  }
}

bool radio_c::receiveData( void ) 
{
  RcData* data = getDataptr();

  if (RadioManager.available())
  {
    // Wait for a message addressed to us from the client
    uint8_t len = sizeof(buf);
    uint8_t from;
    if (RadioManager.recvfromAck(buf, &len, &from)) 
    {   
      lastRecvTime = millis();
      if(!data->setRcData(buf))
      {
        #ifdef DEBUG
          Serial.println("package inclompete")
        #endif
        return false;
      }
      // Send a reply back to the originator client, check for error
      if (!RadioManager.sendtoWait(data->getReturnData(), sizeof(data->getReturnData()), from))
      {
        #ifdef DEBUG
          Serial.println("sendtoWait failed");
        #endif
        return false;
      }
    }  

    // set all analog values to their middle position, if no RC signal is received during 1s!
    if (millis() - lastRecvTime > 1000) 
    { 
      data->setfailsafe();
      #ifdef DEBUG
        Serial.println("No Radio Available - Check Transmitter!");
      #endif
      return false;
    }

    if (millis() - lastRecvTime > 2000) {
      begin(); // re-initialize radio
      lastRecvTime = millis();
      return false;
    }
  }
  return true;
}


void sendSerialCommands( void )
{
  RcData* data = getDataptr();

  if (millis() - lastSerialTime > 20) { // Send the data every 20ms
    lastSerialTime = millis();
    #ifdef DEBUG
      Serial.print('<'); // Start marker
      Serial.println(data->axis1);
      Serial.println(data->axis2);
      Serial.println(data->axis3);
      Serial.println(data->axis4);
      Serial.println(data->pot);
      Serial.println(data->momentary1);
      Serial.println(data->momentary2);
      Serial.println(data->getfailsafe());
      Serial.print('>'); // End marker
    #endif
  }
}