#include "SFBQLED.h"
#include "SFBAssert.h"
#include "SFBWiring.h"   // For ledSet
#include "SFBAlarm.h"    // For Alarms, etc

/* To test:
   export SFB_DIR=../../.. ; g++ -g2 -Wall -pedantic -I"${SFB_DIR}/src/include" -DHOST_MODE -DTEST_SFBQLED SFBQLED.cpp -L ${SFB_DIR}/targets/host/lib -lhw -lreflex -lhw -lreflex -o SFBQLEDtest;./SFBQLEDtest

 */

/**
   A #SFBQLED object provided to control the built-in LEDs without
   blocking.
 */
SFBQLED QLED IN_BODYRAM;

void SFBQLED::initLists() {
  for (u32 i = 0; i < QLED_COUNT; ++i)
    oldest[i] = newest[i] = FLAG;
}

void SFBQLED::initEmpty() {
  queue = 0;
  idleOurs = 0;
  idleOn = 0;
  freeList = FLAG;
  freeCount = 0;
  slots = 0;
  initLists();
}

SFBQLED::SFBQLED() : 
  alarm(0)
{ 
  initEmpty();
}

void SFBQLED::initQueue() {
  API_ASSERT_NONNULL(queue);
  initLists();
  for (u32 i = 0; i < slots; ++i)
    queue[i].next = i+1;
  queue[slots-1].next = FLAG;
  freeList = 0;
  freeCount = slots;
}

u32 SFBQLED::allocEntryIndex() {
  if (freeList == FLAG) return FLAG; // None available
  u32 ret = freeList;
  freeList = queue[freeList].next;
  --freeCount;
  return ret;
}

void SFBQLED::freeEntryIndex(u32 index) {
  queue[index].next = freeList;
  freeList = index;
  ++freeCount;
}

void SFBQLED::begin() {
  const u32 size = 32;
  static u16 buffer[size];
  begin(buffer,size);
}

bool SFBQLED::pop(u32 qled) {
  u32 idx = oldest[qled];
  if (idx==FLAG) return false;
  qentry & e = queue[idx];
  oldest[qled] = e.next;
  if (newest[qled] == idx) 
    newest[qled] = oldest[qled];
  freeEntryIndex(idx);
  return true;
}
void SFBQLED::forget(u32 sfbPin) {
  u32 pin = indexFromSFBPin(sfbPin);
  while (pop(pin)) { }
  makeIdle(pin);
}
u32 SFBQLED::time(u32 sfbPin) {
  u32 pin = indexFromSFBPin(sfbPin);
  u32 totalMs = 0;
  for (u32 idx = oldest[pin]; idx != FLAG;) {
    qentry & e = queue[idx];
    totalMs += (e.onOffTime>>1)<<MS_PER_TICK_BITS;
    idx = e.next;
  }
  return totalMs;
}

void SFBQLED::whenIdle(u32 sfbPin, u32 mode) {
  API_ASSERT_MAX(mode,QLED_IDLE_REFLEX+1);
  u32 mask = 1<<indexFromSFBPin(sfbPin);
  if (mode == QLED_IDLE_REFLEX) {
    idleOurs &= ~mask;
  } else {
    idleOurs |= mask;
    if (mode == QLED_IDLE_ON)
      idleOn |= mask;
    else
      idleOn &= ~mask;
  }
}
void SFBQLED::makeIdle(u32 qled) {
  u32 sfbPin = sfbPinFromIndex(qled);
  if (idleOurs&(1<<qled)) ledSet(sfbPin,idleOn&(1<<qled));
  else pinMode(sfbPin,REFLEX_MODE);
}
void SFBQLED::tick(u32 when) {
  for (u32 i = 0; i < QLED_COUNT; ++i) {
    u32 idx = oldest[i];
    u32 sfbPin = sfbPinFromIndex(i);
    if (idx == FLAG) makeIdle(i);
    else {
      qentry & e = queue[idx];
      ledSet(sfbPin,e.onOffTime&1);
      if (e.onOffTime >= 2)
        e.onOffTime -= 2;
      else pop(i);
    }
  }
  Alarms.set(this->alarm, when+MS_PER_TICK);
}


void SFBQLED::alarmHandler(u32 when, void * arg) {
  SFBQLED * p = (SFBQLED*) arg;
  p->tick(when);
}

void SFBQLED::begin(u16 * buffer, u32 size) {
  API_ASSERT_NONNULL(buffer);
  API_ASSERT_NONZERO(size);
  API_ASSERT_LESS_EQUAL(size,FLAG); // size==FLAG means 0..FLAG-1 is possible index range
  this->queue = (qentry*) buffer;
  this->slots = size;
  this->idleOurs = 0;
  this->idleOn = 0;
  initQueue();
  if (this->alarm == 0)
    this->alarm = Alarms.create(SFBQLED::alarmHandler,this);
  Alarms.set(this->alarm,millis());
}

void SFBQLED::end() {
  for (u32 i = 0; i < QLED_COUNT; ++i) 
    makeIdle(i);
  if (this->alarm)
    Alarms.cancel(this->alarm);
  initEmpty();
}

u32 SFBQLED::indexFromSFBPin(u32 sfbPin) {
  switch (sfbPin) {
  case BODY_RGB_RED_PIN:   return QLED_BODY_RGB_RED_PIN;
  case BODY_RGB_GREEN_PIN: return QLED_BODY_RGB_GREEN_PIN;
  case BODY_RGB_BLUE_PIN:  return QLED_BODY_RGB_BLUE_PIN;
  case NORTH_LED_PIN:      return QLED_NORTH_LED_PIN;
  case SOUTH_LED_PIN:      return QLED_SOUTH_LED_PIN;
  case EAST_LED_PIN:       return QLED_EAST_LED_PIN;
  case WEST_LED_PIN:       return QLED_WEST_LED_PIN;
  case NORTH_SIDE_LED_PIN: return QLED_NORTH_SIDE_LED_PIN;
  case SOUTH_SIDE_LED_PIN: return QLED_SOUTH_SIDE_LED_PIN;
  case EAST_SIDE_LED_PIN:  return QLED_EAST_SIDE_LED_PIN;
  case WEST_SIDE_LED_PIN:  return QLED_WEST_SIDE_LED_PIN;
  default:
    API_BUG(E_API_ARGUMENT);
  }
}

const u8 qledIndexToPin[] = {
  (u8) BODY_RGB_RED_PIN,
  (u8) BODY_RGB_GREEN_PIN,
  (u8) BODY_RGB_BLUE_PIN,
  (u8) NORTH_LED_PIN,
  (u8) SOUTH_LED_PIN,
  (u8) EAST_LED_PIN,
  (u8) WEST_LED_PIN,
  (u8) NORTH_SIDE_LED_PIN,
  (u8) SOUTH_SIDE_LED_PIN,
  (u8) EAST_SIDE_LED_PIN,
  (u8) WEST_SIDE_LED_PIN
};

u8 SFBQLED::sfbPinFromIndex(u32 index) {
  API_ASSERT_MAX(index,QLED_COUNT);
  return qledIndexToPin[index];
}

void SFBQLED::addOp(u32 sfbPin, bool on, u32 ms) { 
  API_ASSERT(queue,E_API_ILLEGAL_STATE); // Must begin() before on/off
  u32 qled = indexFromSFBPin(sfbPin);
  do {
    u32 entry = allocEntryIndex();
    if (entry == FLAG)
      return;                     // No room.  Handle better how?

    if (oldest[qled] == FLAG) {
      newest[qled] = oldest[qled] = entry;
    } else {
      queue[newest[qled]].next = entry;
      newest[qled] = entry;
    }
    u32 ticks = ms>>MS_PER_TICK_BITS;
    if (ticks > MAX_TICKS) {
      ticks = MAX_TICKS;
      ms -= MAX_TICKS<<MS_PER_TICK_BITS;
    } else
      ms = 0;
    queue[entry].onOffTime = (ticks<<1)|(on?1:0);
    queue[entry].next = FLAG;

  } while (ms > 0);

  
}
void SFBQLED::on(u32 sfbPin, u32 ms) { addOp(sfbPin,1,ms); }
void SFBQLED::off(u32 sfbPin, u32 ms) { addOp(sfbPin,0,ms); }
void SFBQLED::set(u32 sfbPin, u32 ms, bool on) { addOp(sfbPin,on?1:0,ms); }


#ifdef TEST_SFBQLED

#include <stdlib.h> /* for exit */
#include <stdio.h>
#include "SFBWiring.h"

volatile u32 timer_1msec_counter;
void die(const char * file, int line, const char * msg) {
  fprintf(stderr,"%s:%d: %s\n",file, line, msg);
  exit(100);
}
#define TEST(condition) do { if (!(condition)) die(__FILE__,__LINE__,#condition); } while (0)

void SFBQLED::assertOK() {
  enum estate {
    UNKNOWN,
    OFF_END,
    FREE,
    ALLOCATED
  };
  u8 entries[256];
  for (u32 i = 0; i < 256; ++i) entries[i] = i<slots?UNKNOWN:OFF_END;
  u8 frees = 0;
  for (u32 i = freeList; i != FLAG; i = queue[i].next) {
    API_ASSERT_EQUAL(entries[i],UNKNOWN);
    entries[i] = FREE;
    ++frees;
  }
  API_ASSERT_EQUAL(available(),frees);
  for (u32 idx = 0; idx < QLED_COUNT; ++idx) {
    u32 last = FLAG;
    for (u32 i = oldest[idx]; i != FLAG; last = i, i = queue[i].next) {
      API_ASSERT_EQUAL(entries[i],UNKNOWN);
      entries[i] = ALLOCATED;
    }
    if (last == FLAG)
      API_ASSERT_EQUAL(oldest[idx],newest[idx]);
    else
      API_ASSERT_EQUAL(last,newest[idx]);
  }
  for (u32 i = 0; i < 256; ++i) 
    API_ASSERT_TRUE(entries[i] != UNKNOWN);
}

void test0() {
  SFBQLED q;
  q.assertOK();

  q.begin();
  q.assertOK();

  q.end();
  q.assertOK();
}

void test1() {
  SFBQLED q;

  q.begin();

  API_ASSERT_EQUAL(32,q.available()); // default event count
  q.assertOK();

  q.on(BODY_RGB_RED_PIN,1000);
  API_ASSERT_EQUAL(31,q.available()); // used one event

  q.assertOK();
  q.off(BODY_RGB_RED_PIN,1000);
  q.assertOK();

  API_ASSERT_GREATER_EQUAL(q.time(BODY_RGB_RED_PIN), 1000+1000-16-16);
  API_ASSERT_LESS_EQUAL(q.time(BODY_RGB_RED_PIN), 1000+1000);

  q.assertOK();
  API_ASSERT_ZERO(q.time(BODY_RGB_BLUE_PIN));
  API_ASSERT_ZERO(q.time(BODY_RGB_GREEN_PIN));

  q.on(BODY_RGB_GREEN_PIN,100);
  API_ASSERT_GREATER_EQUAL(q.time(BODY_RGB_GREEN_PIN), 100-16);
  API_ASSERT_LESS_EQUAL(q.time(BODY_RGB_GREEN_PIN), 100);

  q.off(BODY_RGB_GREEN_PIN,q.time(BODY_RGB_RED_PIN)-q.time(BODY_RGB_GREEN_PIN));
  API_ASSERT_EQUAL(q.time(BODY_RGB_GREEN_PIN), q.time(BODY_RGB_RED_PIN));

  q.on(BODY_RGB_RED_PIN,100);
  q.on(BODY_RGB_GREEN_PIN,100);
  q.on(BODY_RGB_BLUE_PIN,100);
  q.on(NORTH_LED_PIN,100);
  q.on(SOUTH_LED_PIN,100);
  q.on(EAST_LED_PIN,100);
  q.on(WEST_LED_PIN,100);
  q.on(NORTH_SIDE_LED_PIN,100);
  q.on(SOUTH_SIDE_LED_PIN,100);
  q.on(EAST_SIDE_LED_PIN,100);
  q.on(WEST_SIDE_LED_PIN,100);

  q.assertOK();

  q.end();
  q.assertOK();
}

void test2() {
  SFBQLED q;
  const u32 SIZE = 256-1;
  u16 buf[SIZE];

  q.begin(buf,SIZE);
  q.assertOK();

  for (u32 i = 0; i < SIZE; ++i) {
    API_ASSERT_NONZERO(q.available());
    q.on(NORTH_LED_PIN,16);
    q.assertOK();
    API_ASSERT_EQUAL(q.time(NORTH_LED_PIN),16*(i+1));
  }

  API_ASSERT_ZERO(q.available());
  API_ASSERT_EQUAL(q.time(NORTH_LED_PIN),16*SIZE);

  // freeList is now empty.  So another allocation shouldn't change anything
  q.on(NORTH_LED_PIN,16);
  q.assertOK();
  API_ASSERT_EQUAL(q.time(NORTH_LED_PIN),16*SIZE);

  // forgetting unused stuff can't add events
  q.forget(SOUTH_LED_PIN);
  q.assertOK();
  API_ASSERT_ZERO(q.available());

  q.forget(EAST_LED_PIN);
  q.forget(WEST_LED_PIN);
  q.forget(BODY_RGB_RED_PIN);
  q.forget(BODY_RGB_BLUE_PIN);
  q.forget(BODY_RGB_GREEN_PIN);
  q.assertOK();
  API_ASSERT_ZERO(q.available());

  // forgetting used stuff will free up events
  q.forget(NORTH_LED_PIN);
  q.assertOK();
  API_ASSERT_EQUAL(q.time(NORTH_LED_PIN),0);
  API_ASSERT_NONZERO(q.available());

  q.end();
  q.assertOK();
}


void test3() {
  SFBQLED q;

  q.begin();
  q.assertOK();

  q.whenIdle(NORTH_LED_PIN,QLED_IDLE_OFF);
  q.whenIdle(SOUTH_LED_PIN,QLED_IDLE_OFF);
  q.whenIdle(EAST_LED_PIN,QLED_IDLE_OFF);
  q.whenIdle(WEST_LED_PIN,QLED_IDLE_OFF);

  q.on(NORTH_LED_PIN,16);
  q.on(SOUTH_LED_PIN,32);
  q.on(EAST_LED_PIN,48);
  q.on(WEST_LED_PIN,64);

  Alarms.cancel(q.getAlarm());
  Alarms.set(q.getAlarm(),0);
  for (u32 ticks = 0; ticks < 5; ++ticks) {
    u32 ms = ticks*16;
    API_ASSERT_EQUAL(ms,Alarms.get(q.getAlarm()));
    API_ASSERT_EQUAL(q.time(NORTH_LED_PIN),ticks<1?16-ticks*16:0);
    API_ASSERT_EQUAL(q.time(SOUTH_LED_PIN),ticks<2?32-ticks*16:0);
    API_ASSERT_EQUAL(q.time(EAST_LED_PIN),ticks<3?48-ticks*16:0);
    API_ASSERT_EQUAL(q.time(WEST_LED_PIN),ticks<4?64-ticks*16:0);
    q.doTick(ms);

    API_ASSERT_EQUAL(ms<=16,ledIsOn(NORTH_LED_PIN));
    API_ASSERT_EQUAL(ms<=32,ledIsOn(SOUTH_LED_PIN));
    API_ASSERT_EQUAL(ms<=48,ledIsOn(EAST_LED_PIN));
    API_ASSERT_EQUAL(ms<=64,ledIsOn(WEST_LED_PIN));
    q.assertOK();
  }

  q.end();
  q.assertOK();
}

void test4() {
  SFBQLED q;

  q.begin();

  q.whenIdle(BODY_RGB_RED_PIN,QLED_IDLE_OFF);
  q.whenIdle(BODY_RGB_GREEN_PIN,QLED_IDLE_ON);
  q.whenIdle(BODY_RGB_BLUE_PIN,QLED_IDLE_REFLEX);

  q.on(BODY_RGB_RED_PIN,16);
  q.on(BODY_RGB_GREEN_PIN,32);
  q.on(BODY_RGB_BLUE_PIN,48);

  q.off(BODY_RGB_RED_PIN,0);
  q.off(BODY_RGB_GREEN_PIN,0);
  q.off(BODY_RGB_BLUE_PIN,0);

  Alarms.cancel(q.getAlarm());
  Alarms.set(q.getAlarm(),0);
  for (u32 ticks = 0; ticks < 5; ++ticks) {
    u32 ms = ticks*16;
    API_ASSERT_EQUAL(ms,Alarms.get(q.getAlarm()));
    API_ASSERT_EQUAL(q.time(BODY_RGB_RED_PIN),ticks<1?16-ticks*16:0);
    API_ASSERT_EQUAL(q.time(BODY_RGB_GREEN_PIN),ticks<2?32-ticks*16:0);
    API_ASSERT_EQUAL(q.time(BODY_RGB_BLUE_PIN),ticks<3?48-ticks*16:0);
    q.doTick(ms);

    API_ASSERT_EQUAL(ms<=16,ledIsOn(BODY_RGB_RED_PIN));
    API_ASSERT_EQUAL(ms<=32||ms>48,ledIsOn(BODY_RGB_GREEN_PIN));
    API_ASSERT_TRUE(ms<=48?isPinMode(BODY_RGB_BLUE_PIN,OUTPUT):true);
    API_ASSERT_TRUE(ms<=48?ledIsOn(BODY_RGB_BLUE_PIN):isPinMode(BODY_RGB_BLUE_PIN,REFLEX_MODE));
    q.assertOK();
  }

  q.end();
  q.assertOK();
}


const char * progname = "testsfbqled";
bool verbose = 0;
int main() {

  pins_startup_initialization();

  test4();
  test3();
  test2();
  test1();
  test0();
  return 0;
}

#endif
