/*
  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() */

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;
}

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))->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);
}

#include "SFBPrintf.h"
bool SFBMemory::dispatch(int maxPackets,u32 deadline) {
  int hits = 0;

  for (int i = 0; i < maxPackets && IS_EARLIER(millis(), deadline); ++i) {
    while (hits < inUse && hits < maxPackets) {
      u8 index = spinner;
      if (++spinner >= MAX_PACKETS_IN_MEMORY)
        spinner = 0;
      u8 * packet = packets[index];
      if (packet) {
        ++hits;
        killIt = true;
        inTrigger = true;
        Body.trigger(packet,packetSource(packet));
        inTrigger = false;
        if (killIt) {
          forget(index);
        }
      }
    }
  }
  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;
}

