// **********************************************************************************************************
// Moteino gateway/base sketch that works with Moteinos equipped with RFM69 transceiver
// It will buffer the serial data to ensure host serial requests are not missed.
// This is a buffered gateway sketch that receives packets from end node Moteinos, formats them as ASCII strings
//      with the end node [ID] and passes them to Pi/host computer via serial port
//     (ex: "messageFromNode" from node 123 gets passed to serial as "[123] messageFromNode")
// It also listens to serial messages that should be sent to listening end nodes
//     (ex: "123:messageToNode" sends "messageToNode" to node 123)
// Make sure to adjust the settings to match your transceiver settings (frequency, HW etc).
// **********************************************************************************
// Copyright Felix Rusu 2020, http://www.LowPowerLab.com/contact
// **********************************************************************************
#include <RFM69.h>      //get it here: https://github.com/lowpowerlab/rfm69
#include <RFM69_ATC.h>  //get it here: https://github.com/lowpowerlab/RFM69
#include <RFM69_OTA.h>  //get it here: https://github.com/lowpowerlab/RFM69
#include <SPIFlash.h>   //get it here: https://github.com/lowpowerlab/spiflash
#include <PString.h>    //easy string manipulator: http://arduiniana.org/libraries/pstring/
#include <Streaming.h>  //easy C++ style output operators: http://arduiniana.org/libraries/streaming/
//****************************************************************************************************************
//**** IMPORTANT RADIO SETTINGS - YOU MUST CHANGE/CONFIGURE TO MATCH YOUR HARDWARE TRANSCEIVER CONFIGURATION! ****
//****************************************************************************************************************
#define NODEID          1 //the ID of this node
#define NETWORKID     200 //the network ID of all nodes this node listens/talks to
#define FREQUENCY     RF69_915MHZ //Match this with the version of your Moteino! (others: RF69_433MHZ, RF69_868MHZ)
#define ENCRYPTKEY    "sampleEncryptKey" //identical 16 characters/bytes on all nodes, not more not less!
#define IS_RFM69HW_HCW  //required for RFM69HW/HCW, comment out for RFM69W/CW!
//*****************************************************************************************************************************
#define ENABLE_ATC    //comment out this line to disable AUTO TRANSMISSION CONTROL
#define ATC_RSSI      -90  //target RSSI for RFM69_ATC (recommended > -80)
//*****************************************************************************************************************************
// Serial baud rate must match your Pi/host computer serial port baud rate!
#define DEBUG_EN     //comment out if you don't want any serial verbose output
#define SERIAL_BAUD   19200
//*****************************************************************************************************************************
#ifdef DEBUG_EN
  #define DEBUG(input)   {Serial.print(input);}
  #define DEBUGln(input) {Serial.println(input);}
#else
  #define DEBUG(input);
  #define DEBUGln(input);
#endif

#define LED_HIGH digitalWrite(LED_BUILTIN, HIGH)
#define LED_LOW digitalWrite(LED_BUILTIN, LOW)

//******************************************** BEGIN ADVANCED variables ********************************************************************************
#if defined (MOTEINO_M0)
  #define RAMSIZE 16384
#elif defined (MOTEINO_MEGA)
  #define RAMSIZE 16384
#else
  #define RAMSIZE 2048
#endif

#define MAX_BUFFER_LENGTH   25 //limit parameter update requests to 20 chars. ex: Parameter:LongRequest
#define MAX_ACK_REQUEST_LENGTH  30 //60 is max for ACK (with ATC enabled), but need to allow appending :OK and :INV to confirmations from node

typedef struct req {
  uint16_t nodeId;
  char data[MAX_BUFFER_LENGTH]; //+1 for the null terminator
  struct req *next;
}REQUEST;

//dynamically allocated queue (FIFO) data structure
REQUEST* queue = NULL;
byte size_of_queue = 0;

char buff[61]; //61 max payload for radio packets
PString Pbuff(buff, sizeof(buff)); //easy string manipulator
int rssi=0; //signed!
//******************************************** END ADVANCED variables ********************************************************************************
//******************************************** BEGIN GENERAL variables ********************************************************************************
#ifdef ENABLE_ATC
  RFM69_ATC radio;
#else
  RFM69 radio;
#endif

SPIFlash flash(SS_FLASHMEM, 0xEF30); //EF30 for 4mbit Windbond FlashMEM chip
//******************************************** END GENERAL variables ********************************************************************************

void setup() {
  Serial.begin(SERIAL_BAUD);
  radio.initialize(FREQUENCY,NODEID,NETWORKID);
#ifdef IS_RFM69HW_HCW
  radio.setHighPower(); //must include this only for RFM69HW/HCW!
#endif
  radio.encrypt(ENCRYPTKEY);
  
#ifdef ENABLE_ATC
  radio.enableAutoPower(ATC_RSSI);
#endif
  
  char buff[50];
  sprintf(buff, "\nTransmitting at %d Mhz...", radio.getFrequency()/1000000);
  DEBUGln(buff);

  if (flash.initialize())
  {
    DEBUGln("SPI Flash Init OK!");
  }
  else
  {
    DEBUGln("SPI FlashMEM not found (is chip onboard?)");
  }
}

boolean newPacketReceived;
void loop() {
  handleSerialData();   //checks for any serial input from the Pi computer

  //process any received radio packets
  if (radio.receiveDone())
  {
    LED_HIGH;
    rssi = radio.RSSI; //get this asap from transceiver
    if (radio.DATALEN > 0) //data packets have a payload
    {
      for (byte i=9;i<radio.DATALEN;i++) {
        if (radio.DATA[i]=='\n' || radio.DATA[i]=='\r')
          radio.DATA[i]=' '; //remove any newlines in the payload - this should only ever happen with noise data that actually made it through
      }
      Pbuff="";
      Pbuff << '[' << radio.SENDERID << "] " << (char*)radio.DATA;
      Serial << buff << F(" SS:") << rssi << endl; //this passes data to MightyHat / RaspberryPi
    }

    CheckForWirelessHEX(radio, flash, false); //non verbose DEBUG

    //respond to any ACK if requested
    if (radio.ACKRequested())
    {
      REQUEST* aux=queue;
      Pbuff="";
      //walk queue and add pending commands to ACK payload (as many it can fit)
      while (aux!=NULL) {
        if (aux->nodeId==radio.SENDERID)
        {
          //check if payload has room to add this queued command
          if (Pbuff.length() + 1 + strlen(aux->data) <= MAX_ACK_REQUEST_LENGTH)
          {
            if (Pbuff.length()) Pbuff.print(' '); //prefix with a space any previous command in buffer
            Pbuff.print(aux->data); //append command
          }
        }
        aux=aux->next;
      }
      if (Pbuff.length())
        radio.sendACK(buff, Pbuff.length());
      else
        radio.sendACK();
    }
    LED_LOW;
    newPacketReceived = true;
  }
}


boolean insert(uint16_t new_id, char new_data[]) { 
  REQUEST* aux;
  REQUEST* new_node = (REQUEST*)malloc(sizeof(REQUEST));
  if (new_node == NULL) return false;
  new_node->nodeId = new_id; 
  strcpy(new_node->data, new_data);
  new_node->next = NULL;
  if (queue == NULL) queue = new_node;
  else {
      aux = queue;
      while(aux->next != NULL) aux=aux->next;
      aux->next=new_node;
  }
  return true;
}

//processCommand - parse the command and send it to target
//if target is non-responsive it(sleeppy node?) then queue command to send when target wakes and asks for an ACK
//SPECIAL COMMANDS FROM HOST:
// - 123:VOID - removes all queued commands for node 123
// - 123:VOID:command - removes 'command' from queue (if found)
// - REQUESTQUEUE - prints the queued list of nodes on serial port, to host (Pi?)
// - FREERAM - returns # of unallocated bytes at end of heap
// - SYSFREQ - returns operating frequency in Hz
// - UPTIME - returns millis()
void processCommand(char data[], boolean allowDuplicate=false) {
  char *ptr;
  char dataPart[MAX_BUFFER_LENGTH];
  uint16_t targetId;
  byte sendLen = 0;
  ptr = strtok(data, ":");

  if (strcmp(data, "FREERAM")==0)
    Serial << F("FREERAM:") << freeRAM() << ':' << RAMSIZE << endl;
  if (strcmp(data, "REQUESTQUEUE")==0)
    printQueue(queue);
  if (strcmp(data, "SYSFREQ")==0)
    Serial << F("SYSFREQ:") << radio.getFrequency() << endl;
  if (strcmp(data, "UPTIME")==0)
    Serial << F("UPTIME:") << millis() << endl;
  if (strcmp(data, "ENCRYPTKEY")==0)
#ifdef ENCRYPTKEY
    Serial << F("ENCRYPTKEY:") << ENCRYPTKEY << endl;
#else
    Serial << F("ENCRYPTKEY:NONE") << endl;
#endif

  if(ptr != NULL) {                  // delimiter found, valid command
    sprintf(dataPart, "%s", ptr);
    targetId = atoi(dataPart);       // get nodeID part
    ptr = strtok(NULL, "");          // get command part
    sprintf(dataPart, "%s", ptr);

    //check for empty command
    if (strlen(dataPart) == 0) return;

    //check target nodeID is valid
    if (targetId > 0 && targetId != NODEID && targetId<=1023)      
    {
      REQUEST* aux;
      byte removed=0;

      //check if VOID command - if YES then remove command(s) to that target nodeID
      if (strstr(dataPart, "VOID")==dataPart) //string starts with VOID
      {
        //if 'nodeId:VOID' then remove all commands to that node
        //if 'nodeId:VOID:REQUEST' then remove just 'REQUEST' (if found & identical match)
        boolean removeAll=true;
        if (dataPart[4]==':' && strlen(dataPart)>5)
          removeAll=false;

        //iterate over queue
        aux = queue;
        while(aux != NULL)
        {
          if (aux->nodeId==targetId)
          {
            if (removeAll || (!removeAll && strcmp(aux->data, dataPart+5)==0))
            {
              if (aux == queue)
              {
                if (aux->next == NULL)
                {
                  free(queue);
                  queue=NULL;
                  removed++;
                  break;
                }
                else
                {
                  queue = queue->next;
                  free(aux);
                  removed++;
                  aux = queue;
                  continue;
                }
              }
              else
              {
                REQUEST* prev=queue;
                while(prev->next != NULL && prev->next != aux) prev = prev->next; //find previous
                if (prev->next == NULL) break;
                prev->next=prev->next->next;
                free(aux);
                removed++;
                aux=prev->next;
              }
            }
            else aux=aux->next;
          }
          else aux=aux->next;
        }
        DEBUG("DEBUG:VOIDED commands: ");DEBUGln(removed);
        size_of_queue = size_of_queue - removed;
        return;
      }

      //try sending to node, if it fails, continue & add to pending commands queue
      LED_HIGH;
      if (radio.sendWithRetry(targetId, dataPart, strlen(dataPart)))
      {
        LED_LOW;
        return;
      }
      LED_LOW;

      //check for duplicate
      if (!allowDuplicate) {
        //walk queue and check for duplicates
        aux = queue;
        while(aux != NULL)
        {
          //DEBUGln("While");
          if (aux->nodeId==targetId)
          {
            if (strcmp(aux->data, dataPart)==0)
            {
              DEBUGln(F("DEBUG:processCommand() skip (duplicate)"));  
              return;
            }
          }
          aux = aux->next;
        }
      }

      //all checks OK, attempt to add to queue
      if (insert(targetId, dataPart))
      {
        //DEBUG(F("-> inserted: ")); 
        //DEBUG(targetId);
        //DEBUG("_");
        //DEBUGln(dataPart);
        size_of_queue++;
      }
      else 
      {
        DEBUGln(F("DEBUG:INSERT FAIL - MEM FULL!"));
        Serial << F("[") << targetId << F("] ") << dataPart << F(":MEMFULL") << endl;
      }
    }
  }
}

void printQueue(REQUEST* p) {
  if (!size_of_queue) {
    Serial << F("REQUESTQUEUE:VOID") << endl;
    return;
  }

  REQUEST* aux=p;
  while (aux!=NULL) {
    Serial << F("REQUESTQUEUE:") << aux->nodeId << ':' << aux->data << endl;
    aux=aux->next;
  }
}

// here's the processing of single char/bytes as soon as they're coming from UART
void handleSerialData() {
  static char input_line[100]; //static = these get allocated ONCE!
  static byte input_pos = 0;
  if(Serial.available() > 0)
  {
    char inByte = Serial.read();
    switch (inByte)
    {
      case '\r':   //ignore carriage return
        break;

      case '\n':
        if (input_pos==0) break;       // ignore empty lines
        input_line[input_pos] = 0;     // null terminate the string
        DEBUG("DEBUG:handleSerialData:");
        DEBUGln(input_line);
        processCommand(input_line);        // fill up queue
        input_pos = 0; // reset buffer for next time
        break;

      default:
        // keep adding if not full ... allow for terminating byte
        if (input_pos < MAX_BUFFER_LENGTH-1) {
          input_line[input_pos] = inByte;
          input_pos++;
        } else {
          // if theres no EOL coming before MAX_BUFF_CHARS is exceeded we'll just terminate and send it, last char is then lost
          input_line[input_pos] = 0;    // null terminate the string
          DEBUG("DEBUG:MAX_BUFF_CHARS is exceeded - attempting to add (default): ");
          DEBUGln(input_line);
          processCommand(input_line);  //add to queue
          input_pos = 0; //reset buffer for next line
        }
        break;
    }
  }
}

//returns # of unfragmented free RAM bytes (free end of heap)
int freeRAM() {
#ifdef __arm__
  char top;
  return &top - reinterpret_cast<char*>(sbrk(0));
#else
  extern int __heap_start, *__brkval; 
  int v; 
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 
#endif
}