/*
  SFBMemory.cpp - Support for 'Working Memory' - redispatching packets
  Copyright (C) 2009 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA

  $Id$
*/

#include "SFBMemory.h"
#include "SFBHeap.h"
#include "SFBAssert.h"
#include "SFBRandom.h"   /* For random() */
#include "SFBSerial.h"   /* For packetPrefix() */
#include "SFBPrintf.h"   /* For vfacePrintf() */

SFBMemory Memory IN_BODYRAM_BSS = SFBMemory();

SFBMemory::SFBMemory() {
  for (int i = 0; i < MAX_PACKETS_IN_MEMORY; ++i)
    packets[i] = 0;
  spinner = 0;
  inUse = 0;
  PacketPrinterHeader * pph = 
    initializePacketPrinterBuffer(&memoryFacePrinter,WMEM,memoryPacketBuffer,PACKET_PRINTER_PACKET_BUFFER_SIZE,SPINE);
  faceSetPrinter(WMEM, (FacePrinter*) pph);
  facePrintln(WMEM);            // Get sync.  (Needed?)
  lastIndexPrinted = -1;
}

u8 * SFBMemory::makeNewMemory(u32 packetLength) {
  if (inUse >= MAX_PACKETS_IN_MEMORY)
    return 0;
  for (int i = 0; i < MAX_PACKETS_IN_MEMORY; ++i) {
    if (packets[i]==0) {
      packets[i] = heap.makePacket(packetLength);
      if (packets[i]) {
        ((PacketHeader*)(packets[i]-4))->f[PacketHeader::LENGTH] 
          = packetLength;
        ++inUse;
      }
      return packets[i];
    }
  }
  BUG_CODE("",E_BUG_INCONSISTENT_STATE);
}

int SFBMemory::remember(const u8 * packet) {
  API_ASSERT(packet,E_API_NULL_PACKET); // VALID_PACKET will be checked in heap.remember
  if (inUse >= MAX_PACKETS_IN_MEMORY)
    return -1;
  for (int i = 0; i < MAX_PACKETS_IN_MEMORY; ++i) {
    if (packets[i]==0) {
      packets[i] = heap.remember(packet);
      if (packets[i]) {
        ++inUse;
        return i;
      }
      return -1;
    }
  }
  BUG_CODE("",E_BUG_INCONSISTENT_STATE);
}

int SFBMemory::rememberUnread(const u8 * packet,u8 sourceOverride) {
  API_ASSERT(packet,E_API_NULL_PACKET);
  if (inUse >= MAX_PACKETS_IN_MEMORY)
    return -1;
  for (int i = 0; i < MAX_PACKETS_IN_MEMORY; ++i) {
    if (packets[i]==0) {
      packets[i] = heap.remember(packet,true,sourceOverride);
      if (packets[i]) {
        ++inUse;
        return i;
      }
      return -1;
    }
  }
  BUG_CODE("",E_BUG_INCONSISTENT_STATE);
}

bool SFBMemory::fire(const u32 index) {
  u8 * packet = get(index);
  if (!packet) return false;
  killIt = true;
  inTrigger = true;
  Body.trigger(packet,packetSource(packet));
  inTrigger = false;
  if (killIt) {
    forget(index);
  }
  return true;
}

bool SFBMemory::dispatch(int maxPackets,u32 deadline) {
  int hits = 0;

  for (int i = 0; i < maxPackets && IS_EARLIER_OR_EQUAL(millis(), deadline); ++i) {
    while (hits < inUse && hits < maxPackets) {
      u8 index = spinner;
      if (++spinner >= MAX_PACKETS_IN_MEMORY)
        spinner = 0;
      if (fire(index))
        ++hits;
    }
  }
  return hits>0;
}

int SFBMemory::find(const char * prefix, const int length) {
  API_ASSERT(prefix,E_API_NULL_POINTER);
  int len = length;
  if (len < 0) 
    for (len = 0; prefix[len]; ++len) ;
  int index = random(MAX_PACKETS_IN_MEMORY);
  for (int count = 0; count < MAX_PACKETS_IN_MEMORY; ++count) {
    if (packets[index] && packetPrefix(packets[index],(const u8 *) prefix,len))
      return index;
    if (++index >= MAX_PACKETS_IN_MEMORY)
      index = 0;
  }
  return -1;
}

bool SFBMemory::forget(u32 index) {
  API_ASSERT_MAX(index,MAX_PACKETS_IN_MEMORY);
  if (packets[index]==0) return false;
  heap.free(packets[index]);
  packets[index] = 0;
  --inUse;
  return true;
}

void SFBMemory::MemoryFacePrint(u8 face, u8 ch) { Memory.facePrint(face,ch); }
void SFBMemory::MemoryFacePrintln(u8 face) { Memory.facePrintln(face); }

const FacePrinter SFBMemory::memoryFacePrinter = {
  SFBMemory::MemoryFacePrint,
  SFBMemory::MemoryFacePrintln,
  0,
  0
};

void SFBMemory::facePrint(u8 face, u8 ch) {
  API_ASSERT_EQUAL(WMEM,face);
  lastIndexPrinted = -1;        // Flag no complete packet
  PacketPrinterHeader * pph = (PacketPrinterHeader*) faceGetPrinter(face);
  API_ASSERT(pph,E_BUG_INCONSISTENT_STATE); // Shouldn't be able to get here otherwise
  packetPrinterStoreDataByte(pph, ch);      // Stash the byte in the memory packet buffer
}

void SFBMemory::facePrintln(u8 face) {
  API_ASSERT_EQUAL(WMEM,face);
  PacketPrinterHeader * pph = (PacketPrinterHeader*) faceGetPrinter(face);
  API_ASSERT(pph,E_BUG_INCONSISTENT_STATE); // Shouldn't be able to get here otherwise
  packetPrinterTerminatePacket(pph);

  u8 * packet = packetPrinterPrintedPacket(pph); // Get ptr to printed packet
  lastIndexPrinted = (s8) Memory.remember(packet);      // Remember it for later
}

int SFBMemory::printf(const char * format, ...) {
  va_list ap;
  va_start(ap, format);
  vfacePrintf(WMEM, format, ap);
  return lastIndexPrinted;
}
