#include "xRadioLib.h"
//#include <RFM69registers.h>
#include <SPI.h>
#include <LowPower.h> // to get sleep code

SPIFlash flash(FLASH_SS, 0xEF30); //EF30 for 4mbit  Windbond chip (W25X40CL)

#if 1
  #define DEBUG(input)   {Serial.print(input);}
  #define DEBUGln(input) {Serial.println(input);}
#else
  #define DEBUG(input);
  #define DEBUGln(input);
#endif


xRadioLib::xRadioLib(){
    //setup default values in init
}

xRadioLib::~xRadioLib(){/*nothing to destruct*/}

void xRadioLib::setRadioInHubMode() {
  _hubMode = true;
  DEBUGln("*** Hub / Gateway setup ***");
}
  

void xRadioLib::initWithDefaults() {
    _gatewayId = 1;
//      // Hard Reset the RFM module
//    pinMode(RFM69_RST, OUTPUT);
//    digitalWrite(RFM69_RST, HIGH);
//    delay(100);
//    digitalWrite(RFM69_RST, LOW);
//    delay(100);
 
    _radio.initialize(_frequency,_nodeId,_networkId);
    DEBUGln("*** init radio done ***");
    if (_is_Rfm69Hw) {
        DEBUGln("Setting up high power");
        _radio.setHighPower();
        _radio.setPowerLevel(51);     // start with power at MAX
    }
    _radio.enableAutoPower(-59);
    _radio.encrypt(_ENCRYPTKEY);

    //_radio.readAllRegs();

    if (!_hubMode)
      //_radio.sleep(); //sleep right away to save power
 DEBUGln("*** before flash***");
//    if (flash.initialize()) {
//      flash.sleep();
//      isFlashPresent=true;
//   }
//   else DEBUGln("no flash");
   
    DEBUG( "Initializing Radio NodeId:" );DEBUG( _nodeId );DEBUG(" and NetworkId: "); DEBUGln(_networkId);
    char buff[50];
    sprintf(buff, "\nTransmitting at %d Mhz...", _frequency==RF69_433MHZ ? 433 : _frequency==RF69_868MHZ ? 868 : 915);
    DEBUGln(buff);
}

void xRadioLib::init(uint8_t node, uint8_t network, uint8_t freq) {
    _frequency= freq;
    _nodeId   = node;
    _networkId= network;
    
    this->initWithDefaults();

    //if we're not the hub just send a hello to hub for test purposes
    if (!_hubMode) {
      if (this->sendToHub("S:0*M:Hello I just booted", 25)) 
      //if (_radio.sendWithRetry(1, (byte*)F("S:0*M:Hello I just booted"), 25, _nbRetriesRadioSend, 40)) //S:0 to tell hub we're going to sleep since we're a node
        Serial.println(" ok!");
      else 
        Serial.println(" nothing...");
    }
}

bool xRadioLib::sendToHub(char *sendBuffer, byte len) {
  //DEBUG(" Sending to Gateway Id:");DEBUGln(_gatewayId);
  DEBUG(" Sending to Gateway: ");DEBUGln(sendBuffer);
   bool ack = _radio.sendWithRetry(_gatewayId, (byte*)sendBuffer, len, _nbRetriesRadioSend, 40); //retry 60 times, wait 50ms total 3s
  //always go back to sleep after sending?
  //_radio.sleep();
  return ack;
}

bool xRadioLib::sendCmdToNode(int node, char *sendBuffer, byte len) {
  //sendBuffer[len]=0; //not a good idea if you send const strings
  //DEBUGln(sendBuffer);
   bool ack = _radio.sendWithRetry(node, sendBuffer, len, _nbRetriesRadioSend, 40); //retry 25 times, wait 40ms
  //always go back to sleep after sending?
  _radio.sleep();
  return ack;
}

uint8_t xRadioLib::sleepUntilPacket(bool goToSleepDirect)
{
  #if NO_SLEEP_FOR_U
    Serial.println("Going to sleep...");
    Serial.flush();
    _radio.receiveDone();delay(20);
    LowPower.powerDown(SLEEP_FOREVER, ADC_OFF, BOD_OFF);     
    Serial.println("Woke up!!!");
    Serial.flush();delay(10);
   #else
  uint8_t len = 0;

  if (!goToSleepDirect) {
    DEBUGln("In Listening mode now..."); 
    _radio.clearListenBuffer();              // we can clear the buffer now since we HAVE to be done with it!
    _radio.startListening(_buffer,_bufferSize);  // put radio into listen mode
    DEBUGln("In Listening mode REALLY..."); 
    Serial.flush();  
    // flush any residual stuff in Serial port
  }
    DEBUGln("In Listening mode REALLY 3..."); 
    Serial.flush(); 
  //if (isFlashPresent)
  //  flash.sleep();
    DEBUGln("In Listening mode 4..."); 
    DEBUGln("before flush..."); 
    Serial.flush(); 
  // DON'T PUT RADIO TO SLEEP!  IT'S IN LISTENING LOW POWER MODE!
  LowPower.powerDown(SLEEP_FOREVER, ADC_OFF, BOD_OFF);     //  go to sleep until we get woken up by a burst from Gateway
  

  // WOKEN UP!
  len = _radio.listenReceivedBytes(); // see what we have (if anything)
  //DEBUGln(len); 
  if (len > 0 ) {
  Serial.flush();  
  DEBUGln("Awake now!"); 

  //terminate string so strcpy and shit works
  _buffer[len]=0;

   //DEBUGln((char*)_buffer);  
     
  // we've woken up!  Must mean we've got a command to wake up and resume!
  _radio.endListening();

  _radio.restoreInit(_frequency,_nodeId,_networkId);  // restore radio to normal settings
  _radio.encrypt(_ENCRYPTKEY);
  //_radio.sleep();  // put radio to sleep while we wait for our time slot

  //give time to the hub to come back from the burst (currently coded at 3s)
  delay(3000);

  return len;
  } else //go back to sleep we probably woke up from interrupt
    sleepUntilPacket(true);
#endif
}

uint8_t xRadioLib::getLatestCommand(byte *b, int *rssi) {
  //console.log("Getting latest cmd");
  //b = _buffer; //should we do a copy instead?
  strcpy( (char*)b,(char *)_buffer);
  *rssi= _latestSenderRSSI;
  return _latestNodeId;
}

void xRadioLib::wakeUpNodeWithCmd( uint8_t targetNode,byte *cmd, byte len )
{
  //RFM69Listener::send( _radio, targetNode, _buffer, _bufferSize, 2000, false );
  DEBUG("Sending burst to:"); DEBUGln(targetNode); Serial.flush();  // make sure this is flushed before sending
  //digitalWrite(LED,HIGH);
  _radio.sendBurst(targetNode, cmd, len);  // send the burst
  _radio.restoreInit(_frequency,_nodeId,_networkId);
  _radio.encrypt(_ENCRYPTKEY);
}


uint8_t xRadioLib::receivedBytesFromListenningMode() {
    //return RFM69Listener::receivedBytes();
    uint8_t len =0;
     if (_radio.receiveDone())
      {
        DEBUG("#[");
        DEBUG(++_packetCount);
        DEBUG(']');
        DEBUG('[');DEBUG(_radio.SENDERID);DEBUG("]");

        // ****************************
        //check for wireless programming
        if (isFlashPresent && _radio.DATALEN >= 4 && _radio.DATA[0]=='F' && _radio.DATA[1]=='L' && _radio.DATA[2]=='X' && _radio.DATA[3]=='?') {
          DEBUG("Wireless Programming acquired it seems"); 
          //CheckForWirelessHEX(_radio, flash, true);
        } else {
         // ****************************
        //Regular message code - send back len
        _latestNodeId     = _radio.SENDERID;
        len = _radio.DATALEN;
        
        if (_promiscuousMode)
        {
          DEBUG("PM to [");DEBUG(_radio.TARGETID);DEBUG("] ");
        }
        for (byte i = 0; i < _radio.DATALEN; i++)
          DEBUG((char)_radio.DATA[i]);
        _latestSenderRSSI = _radio.RSSI;
        DEBUG("   [RX_RSSI:");DEBUG(_radio.RSSI);DEBUG("]");
        //DEBUGln(_latestSenderRSSI);

        //DEBUGln(len);
        //store our own len since the ackRequested will set it back to 0!
        
        //copy data from rtadio to our buffer
        //maybe check that we dont over run the buffer?
        if ( len > _bufferSize)
          len = _bufferSize;
          
        memcpy( (char*)_buffer,(char *) _radio.DATA, len );
        _buffer[len]=0;
        
        if (_radio.ACKRequested())
        {
          _radio.sendACK();
          DEBUGln(" - ACK sent.");
        }

      DEBUG("Returning len="); DEBUGln(len); 
      delay(20);
      return len;
      }
      }
}
void xRadioLib::setHighPower(bool power) {
    _is_Rfm69Hw = power;
}