#include "archinc.h"
#include "board.h"
#include "eventq.h"

static unsigned int maxEvents = 0;
static unsigned int maxUrgentEvents = 0;

static unsigned int eventsDropped = 0;
static unsigned int urgentEventsDropped = 0;

static unsigned int bogusEvents = 0;

static unsigned char eventQueue[CONFIG_EVENTQ_MAX_EVENTS];
static unsigned char urgentEventQueue[CONFIG_EVENTQ_MAX_URGENT_EVENTS];

// TODO: make this into a const table to move out of data seg
static eventHandler eventFuncs[CONFIG_EVENTQ_MAX_HANDLERS];

static int qHead = 0;
static int qTail = 0;
static int qSize = 0;

static int qUrgentHead = 0; 
static int qUrgentTail = 0;
static int qUrgentSize = 0;

int eventqInit(void) 
{
  int i;
  
  for (i=0; i<CONFIG_EVENTQ_MAX_HANDLERS; i++)
    eventFuncs[i] = NULL;
    
  for (i=0; i<CONFIG_EVENTQ_MAX_EVENTS; i++)
    eventQueue[i] = 0;
    
  for (i=0; i<CONFIG_EVENTQ_MAX_URGENT_EVENTS; i++)
    urgentEventQueue[i] = 0;
    
  return 0;
}

void eventqPut(int addEvent)
{
  if (qSize == CONFIG_EVENTQ_MAX_EVENTS) {
    eventsDropped++;
    return;
    }

  // IRQ_DISABLE();
  eventQueue[qHead] = (unsigned char)addEvent;
  circInc(qHead, 0, (CONFIG_EVENTQ_MAX_EVENTS-1));
  qSize++;
  // IRQ_ENABLE();
  if (qSize > maxEvents) {
    maxEvents = qSize;
    }
}

void eventqUrgentPut(int addUrgentEvent)
{
  if (qUrgentSize == CONFIG_EVENTQ_MAX_URGENT_EVENTS) {
    eventqPut(addUrgentEvent); // queue the event in the normal queue
    urgentEventsDropped++;
    return;
    }

  // IRQ_DISABLE();
  urgentEventQueue[qUrgentHead] = (unsigned char)addUrgentEvent;
  circInc(qUrgentHead, 0, (CONFIG_EVENTQ_MAX_URGENT_EVENTS-1));
  qUrgentSize++;
  // IRQ_ENABLE();

  if (qUrgentSize > maxUrgentEvents) {
    maxUrgentEvents = qUrgentSize;
    }    
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
int INLINE_KEYWORD eventqGet(void)
{
  int retVal = -1;

  if (qUrgentSize > 0) {
    retVal = (int)urgentEventQueue[qUrgentTail];
    circInc(qUrgentTail, 0, (CONFIG_EVENTQ_MAX_URGENT_EVENTS-1));
    IRQ_DISABLE();
    qUrgentSize--;
    IRQ_ENABLE();
    }
  else if (qSize > 0) {
    retVal = (int)eventQueue[qTail];
    circInc(qTail, 0, (CONFIG_EVENTQ_MAX_EVENTS-1));
    IRQ_DISABLE();
    qSize--;
    IRQ_ENABLE();
    }
  else {
    return retVal;
    }

  return retVal;
}
 
int eventqRegisterHandler(int evType, eventHandler handler)
{
  if ((evType < 0) || (evType >= CONFIG_EVENTQ_MAX_HANDLERS))
    return -1;

  eventFuncs[evType] = handler;
    
  return 0;
}

int eventqGetHandler(int evType, eventHandler *pHandler)
{
  if ((evType < 0) || (evType >= CONFIG_EVENTQ_MAX_HANDLERS))
    return -1;

  if (pHandler == NULL)
    return -1;
  
  *pHandler = eventFuncs[evType];
  
  return 0;
}

// the event queue dispatcher

void eventqDispatch(void)
{
  int currentEvent = eventqGet();

  while (currentEvent != -1) {
#ifdef CONFIG_EVENTQ_BUSY_LED
    boardSetBusyLed(true);
#endif
    // dispatch event
    if ((currentEvent >=0) && (currentEvent < CONFIG_EVENTQ_MAX_HANDLERS)) {
      if (eventFuncs[currentEvent] != NULL) {
        (eventFuncs[currentEvent])();
        }
      }
    else {
      bogusEvents++;
    }
    
    currentEvent = eventqGet();
    }
#ifdef CONFIG_EVENTQ_BUSY_LED
  boardSetBusyLed(false);
#endif

} // eventqDispatch()