#include <FragmentedMessage.h>

#include <SPI.h>
#include <TemperaturePacket.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <RF24_config.h>
//#include "printf.h"

const int RF24_CE = 9;
const int RF24_CSN = 10;

RF24 radio(RF24_CE, RF24_CSN);

const uint64_t READ_PIPE_0 = 0x0000000000LL;
const uint64_t READ_PIPE_1 = 0x0000000001LL;
const uint64_t READ_PIPE_2 = 0x0000000002LL;
const uint64_t READ_PIPE_3 = 0x0000000003LL;
const uint64_t READ_PIPE_4 = 0x0000000004LL;
const uint64_t READ_PIPE_5 = 0x0000000005LL;

const int MAX_INCOMING_MESSAGES = 6;
FragmentedMessage* IncomingMessages[MAX_INCOMING_MESSAGES];

const int PACKET_SIZE = 32;

void setup()
{
  Serial.begin(57600);
  radio.begin();
  radio.setRetries(15,15);
  radio.openReadingPipe(0, READ_PIPE_0);
  radio.openReadingPipe(1, READ_PIPE_1);
  radio.openReadingPipe(2, READ_PIPE_2);
  radio.openReadingPipe(3, READ_PIPE_3);
  radio.openReadingPipe(4, READ_PIPE_4);
  radio.openReadingPipe(5, READ_PIPE_5);
  radio.startListening();
  //radio.printDetails();
}

void loop()
{
  processPackets();
}

void processPackets()
{
  //check_mem();
  //printFragmentInfo();
  //printMemoryInfo();
  while(!radio.available())
  {
    delay(100);
  }
  
  bool done = false;

  while(!done)
  { 
   char radioPacket[PACKET_SIZE];
   memset(radioPacket,0,PACKET_SIZE);
   done = radio.read( &radioPacket, PACKET_SIZE );
   //Serial.print(radioPacket);
   //DebugPrintMessageFragment(radioPacket);   
      
   char* defragmentedMessage = DefragmentMessage(radioPacket);
   if(defragmentedMessage != NULL)
   {
     Serial.println(defragmentedMessage);
     delete[] defragmentedMessage;
   }
  }
}

char* DefragmentMessage(char* messageFragment)
{     
  int packetSender = FragmentedMessage::DecodeFragmentSenderID(messageFragment);
  
  if((packetSender < 0) || (packetSender >= MAX_INCOMING_MESSAGES))
  {
    Serial.print("Remark,Invalid Sender,");
    Serial.println(packetSender);
    return NULL;
  }
  
  FragmentedMessage* fragmentedMessage = IncomingMessages[packetSender];
  if(fragmentedMessage == NULL)
  {
    fragmentedMessage = new FragmentedMessage();
    IncomingMessages[packetSender] = fragmentedMessage;
  }
  
  fragmentedMessage->AppendMessageFragment(messageFragment);
  
  if(fragmentedMessage->HasChecksumFragment())
  {
   char* defragmentedMessage = fragmentedMessage->DefragmentMessage();
   IncomingMessages[packetSender] = NULL;
   delete fragmentedMessage;
   return defragmentedMessage;
  }
  
  return NULL;     
}

void check_mem() {
  extern int __heap_start, *__brkval; 
  int v; 
  int freemem = (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 

  Serial.print("Remark,Free Memory,");
  Serial.println(freemem);
}


  void DebugPrintMessageFragment(char* messageFragment)
  {
     int senderID = FragmentedMessage::DecodeFragmentSenderID(messageFragment);
     int fragmentNumber = FragmentedMessage::DecodeFragmentNumber(messageFragment);
     int fragmentChecksum = FragmentedMessage::DecodeFragmentChecksum(messageFragment);
     
     Serial.print("Sender: ");
     Serial.print(senderID);
     Serial.print(" Fragment: ");
     Serial.print(fragmentNumber);
     Serial.print(" Checksum: ");
     Serial.println(fragmentChecksum);
     
     Serial.print("[");
    for(int i = 2; i < 32; i++)
    {
      Serial.print((char)messageFragment[i]);
    }
     Serial.print("]");    
    Serial.println("");
  }
  
  
void printFragmentInfo()
{
 for(int messageIndex = 0; messageIndex < MAX_INCOMING_MESSAGES; messageIndex++)
 {
   Serial.print("|");
   if(IncomingMessages[messageIndex] == NULL)
   {
    Serial.print("NULL"); 
   }
   else
   {
     Serial.print(IncomingMessages[messageIndex]->NumFragments);
     Serial.print(":");
     
    for(int fragmentIndex =0; fragmentIndex < IncomingMessages[messageIndex]->NumFragments; fragmentIndex++)
    {
     if(IncomingMessages[messageIndex]->Fragments[fragmentIndex] == NULL)
     {
        Serial.print("N");
     }
     else
     {
       Serial.print("O");
     }     
    }
    
    Serial.print(",");
    Serial.print(IncomingMessages[messageIndex]->HasChecksumFragment() ? "O":"N");
   }
 }
 Serial.println();
}
  
  
  
  
  
  
  
  
  
  
  typedef struct __freelist {
  size_t sz;
  struct __freelist *nx;
} FREELIST;
 
extern FREELIST *__flp;
extern char *__brkval;
 
 
/**
 * Get the total memory used by your program. The total will
 * include accounting overhead internal to the library
 */
 
size_t getMemoryUsed()
{
  size_t used;
  FREELIST *fp;
 
// __brkval=0 if nothing has been allocated yet
 
  if(__brkval==0)
    return 0;
 
// __brkval moves up from __malloc_heap_start to
// __malloc_heap_end as memory is used
 
  used=__brkval-__malloc_heap_start;
 
// memory free'd by you is collected in the free list and
// compacted with adjacent blocks. This, combined with malloc's
// intelligent picking of candidate blocks drastically reduces
// heap fragmentation. Anyway, since blocks in the free list
// are available to you at no cost we need to take them off.
 
  for(fp=__flp;fp;fp=fp->nx)
    used-=fp->sz+sizeof(size_t);
 
  return used;
}
 
 
/**
 * Get the total free bytes
 */
 
size_t getFreeMemory()
{
  return (size_t)AVR_STACK_POINTER_REG-
         (size_t)__malloc_margin-
         (size_t)__malloc_heap_start-
         getMemoryUsed();
}
 
 
/**
 * Get the largest available block that can be successfully
 * allocated by malloc()
 */
 
size_t getLargestAvailableMemoryBlock()
{
  size_t a,b;
 
  a=getLargestBlockInFreeList();
  b=getLargestNonFreeListBlock();
 
  return a>b ? a : b;
}
 
 
/**
 * Get the largest block in the free list
 */
 
size_t getLargestBlockInFreeList()
{
  FREELIST *fp;
  size_t maxsize=0;
 
  for(fp=__flp;fp;fp=fp->nx)
    if(fp->sz>maxsize)
      maxsize=fp->sz;
 
  return maxsize;
}
 
 
/**
 * Get the number of blocks in the free list
 */
 
int getNumberOfBlocksInFreeList()
{
  FREELIST *fp;
  int i;
 
  for(i=0,fp=__flp;fp;fp=fp->nx,i++);
  return i;
}
 
 
/**
 * Get total size of free list (includes library overhead)
 */
 
size_t getFreeListSize()
{
  FREELIST *fp;
  size_t size;
 
  for(size=0,fp=__flp;fp;fp=fp->nx,size+=fp->sz+sizeof(size_t));
  return size;
}
 
 
/**
 * Get the largest block that can be successfully allocated
 * without reuse from the free list
 */
 
size_t getLargestNonFreeListBlock()
{
  char *cp,*brkval;
 
// this code is an adapted fragment from malloc() itself
 
  brkval=__brkval == 0 ? __malloc_heap_start : __brkval;
 
  if((cp=__malloc_heap_end)==NULL)
    cp=(char *)AVR_STACK_POINTER_REG-__malloc_margin;
  if(cp<=brkval)
    return 0;
 
  return cp-brkval;
}


void printMemoryInfo()
{
  Serial.print("Used: ");
  Serial.print(getMemoryUsed());
  Serial.print(" Free: ");
  Serial.print(getFreeMemory());
  Serial.print(" LAvail: ");
  Serial.print(getLargestAvailableMemoryBlock());
  Serial.print(" LBlock: ");
  Serial.print(getLargestBlockInFreeList());
  Serial.print(" NumBlock: ");
  Serial.print(getNumberOfBlocksInFreeList());
  Serial.print(" FLS: ");
  Serial.print(getFreeListSize());
  Serial.print(" NFLB: ");
  Serial.println(getLargestNonFreeListBlock());
}