/*
  SFBAlarm.cpp - Main SFBAlarm-level operational control
  Copyright (C) 2008 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
*/

/*
 * This is a conventional heap-based priority queue implementation,
 * using an indirect index array 'slots' to reorder elements without
 * copying the element info, and a reverse array 'idxs' to find the
 * current priority queue position of an element, to support deletion
 * and priority changes.
 *
 *    used      alarms
 *    +----+    +----+
 *    |  3 |    |  4 |
 *    +----+    +----+
 *             alarm
 *     slots   Number idxs     times      handlers     args
 *     +----+        +----+ +---------+ +---------+ +---------+
 *  0  | xx |     0  | 0  | |   --    | |   --    | |   --    |
 *     +----+        +----+ +---------+ +---------+ +---------+
 *  1  | 3  |     1  | 2  | |     210 | | MyAlarm | |   0     |
 *     +----+        +----+ +---------+ +---------+ +---------+
 *  2  | 1  |     2  | 3  | |     309 | |  Serial | |&SerPort1|
 *     +----+        +----+ +---------+ +---------+ +---------+
 *  3  | 2  |     3  | 1  | |     123 | |  Serial | |&SerPort0|
 *     +----+        +----+ +---------+ +---------+ +---------+
 *  4  | 4  |     4  | 0  | |    ??   | |    ??   | |    ??   |
 *     +----+        +----+ +---------+ +---------+ +---------+
 *
 * Mapping invariants:
 *  (i >= 1 && i <= used) => idxs[slots[i]] == i
 *
 *  (used <= alarms)   - You can't have more events scheduled than are defined..
 *
 * (There's also the priority queue invariant, of course..)
 *
 */

#include "SFBAlarm.h"
#include "SFBTypes.h"
#include "SFBAssert.h"
#include "SFBWiring.h"  /* for millis() */

#ifdef TEST_SFBALARM
#include <stdlib.h> /* for exit */
void _apiError_(u32 code,const char * file, int lineno) { exit(code); }
#endif

SFBAlarm Alarms IN_BODYRAM_BSS;

u32 SFBAlarm::create(SFBAlarmHandler handler) { /* Create a new alarm object, return alarm number > 0 or 0 if full */ 
  return create((SFBAlarmArgHandler) handler, (void*) 0);
}

void SFBAlarm::deleteElt(int slot)
{
  int alarmIdx = slots[slot];
  u32 oldTime = times[alarmIdx];

  idxs[alarmIdx] = 0;           /* Disrupt mapping invariant */
  int hadUsed = used--;

  if (hadUsed > 1 && slot != hadUsed) {

    slots[slot] = slots[hadUsed];
    idxs[slots[slot]] = slot;

    slots[hadUsed] = alarmIdx;

    if (IS_EARLIER(times[slots[slot]],oldTime))
      bubbleUp(slot);
    else
      bubbleDown(slot);
  }
}

void SFBAlarm::bubbleUp(int fromIndex)
{
  int tmpIdx = slots[fromIndex];
  while (fromIndex > 1 && IS_EARLIER(times[tmpIdx],times[slots[fromIndex>>1]])) {
    int nextIndex = fromIndex>>1;
    slots[fromIndex] = slots[nextIndex]; /* Disrupt/fix invariant */
    idxs[slots[fromIndex]] = fromIndex;  /* Partially restore invariant */
    fromIndex = nextIndex;
  }
  slots[fromIndex] = tmpIdx;    /* Place in final position */
  idxs[tmpIdx] = fromIndex;     /* Fix final piece of invariant */
}

void SFBAlarm::bubbleDown(int k)
{
  int topIdx = slots[k];
  while (k <= used/2) {
    int j = k+k;
    if (j < used && IS_EARLIER(times[slots[j+1]],times[slots[j]])) ++j;
    if (IS_EARLIER_OR_EQUAL(times[topIdx],times[slots[j]])) break;
    slots[k] = slots[j];        /* Disrupt/fix invariant */
    idxs[slots[k]] = k;         /* Partially restore invariant (slots[j] still wrong) */
    k = j;
  }
  slots[k] = topIdx;            /* Place in final position */
  idxs[topIdx] = k;             /* Fix final piece of invariant */
}

u32 SFBAlarm::create(SFBAlarmArgHandler handler, void * arg) {
  API_ASSERT(handler,E_API_NULL_HANDLER);
  API_ASSERT(canCreate(),E_API_NO_MORE_ALARMS);

  u32 alarmIdx = ++alarms;
  idxs[alarmIdx] = 0;        /* Not scheduled */

  times[alarmIdx] = 0;
  handlers[alarmIdx] = handler;
  args[alarmIdx] = arg;

  return alarmIdx;
}

void SFBAlarm::update(u32 alarmNumber, SFBAlarmArgHandler handler, void * arg) {
  API_ASSERT(alarmNumber>0 && alarmNumber<MAX_EVENTS, E_API_BAD_ALARM);
  API_ASSERT(handler,E_API_NULL_HANDLER);

  handlers[alarmNumber] = handler;
  args[alarmNumber] = arg;
}

u32 SFBAlarm::get(u32 alarmNumber) {
  API_ASSERT(alarmNumber>0 && alarmNumber<MAX_EVENTS, E_API_BAD_ALARM);
  API_ASSERT(handlers[alarmNumber], E_API_BAD_ALARM);

  return times[alarmNumber];    // Either current OR LAST scheduled time, or 0
}

bool SFBAlarm::set(u32 alarmNumber, u32 when) {
  API_ASSERT(alarmNumber>0 && alarmNumber<MAX_EVENTS, E_API_BAD_ALARM);
  API_ASSERT(handlers[alarmNumber], E_API_BAD_ALARM);

  if (idxs[alarmNumber]==0) {   /* Not currently scheduled */ 
    u32 s = ++used;
    slots[s] = alarmNumber;
    idxs[alarmNumber] = s;
    times[alarmNumber] = when;
    bubbleUp(used);
    return false;
  }

  u32 oldTime = times[alarmNumber];
  times[alarmNumber] = when;
  u32 s = idxs[alarmNumber];
  if (oldTime < when)
    bubbleDown(s);
  else if (oldTime > when) 
    bubbleUp(s);
  return true;
}

bool SFBAlarm::cancel(u32 alarmNumber) {
  API_ASSERT(alarmNumber>0 && alarmNumber<MAX_EVENTS, E_API_BAD_ALARM);
  API_ASSERT(handlers[alarmNumber], E_API_BAD_ALARM);

  if (idxs[alarmNumber] == 0)
    return false;

  deleteElt(idxs[alarmNumber]);

  return true;

}

SFBAlarm::SFBAlarm() {
  for (int i = 0; i < MAX_EVENTS; ++i) {
    slots[i] = i;
  }
  used = 0;
  alarms = 0;
  currentAlarmIndex = 0;
}

bool SFBAlarm::canCreate() { 
  return alarms < MAX_EVENTS-1; 
}

bool SFBAlarm::canAdd() { 
  return used < MAX_EVENTS-1; 
}

bool SFBAlarm::canRemove() { 
  return used > 0; 
}

int SFBAlarm::runDueEvents(int maxEvents, u32 deadline) { 
  int events = 0;
  for (int i = 0; i < maxEvents && IS_EARLIER_OR_EQUAL(millis(),deadline); ++i)
    events += runPriorityEvents(1, millis()); 
  return events;
}

void SFBAlarm::removeAndRunTop() {
  if (!canRemove()) return;

  int slot = slots[1];

  deleteElt(1);

  if (handlers[slot] != 0) {
    currentAlarmIndex = slot;
    handlers[slot](times[slot], args[slot]);
    currentAlarmIndex = 0;
  }

}

int SFBAlarm::runPriorityEvents(int maxEvents, u32 now) {
  int events = 0;
  while (events < maxEvents && canRemove() && IS_EARLIER_OR_EQUAL(getTopTime(),now)) {
    removeAndRunTop();
    ++events;
  }

  return events;
}

#ifdef TEST_SFBALARM

/* Compilation for testing, e.g.:
g++ -DTEST_SFBALARM -O0 -g3 -Wall -pedantic -Werror -Wundef    -I../../../src/include  -DHOST_MODE -MMD -MP -MF"../../../targets/host/build/SFBAlarm.d" -MT"../../../targets/host/build/SFBAlarm.d" -o"./sfbalarmtest" "SFBAlarm.cpp";valgrind ./sfbalarmtest
 */
#include <stdio.h>
#include <stdlib.h>
volatile u32 timer_1msec_counter;
int die(const char * message) {
  printf("%s\n",message);
  exit(1);
}
#define ASSERT(condition) ((condition)?0:die(#condition))

void heapcheck(int i) {
  int used = Alarms.used;
  if (i > used) return;
  int pslot = Alarms.slots[i];
  ASSERT(Alarms.handlers[pslot]);

  ASSERT(Alarms.idxs[pslot]==i);

  if (i*2 > used) return;
  int lslot = Alarms.slots[i*2];
  ASSERT(IS_EARLIER_OR_EQUAL(Alarms.times[pslot],Alarms.times[lslot]));
  ASSERT(Alarms.handlers[lslot]);
  heapcheck(i*2);

  if (i*2+1 > used) return;
  int rslot = Alarms.slots[i*2+1];
  ASSERT(IS_EARLIER_OR_EQUAL(Alarms.times[pslot],Alarms.times[rslot]));
  ASSERT(Alarms.handlers[rslot]);
  heapcheck(i*2+1);
}

#define makeHandler(whenVal,argVal)                    \
static void handler##whenVal##_##argVal(u32 when, void * arg) { \
  ASSERT(when==whenVal);                               \
  ASSERT((int)arg==argVal);                            \
}
makeHandler(10,20)
makeHandler(5,6)
void runRandoms();
void runTests() {
  heapcheck(1);
  ASSERT(Alarms.getUsed()==0);
  ASSERT(Alarms.canAdd());
  ASSERT(!Alarms.canRemove());
  int an1 = Alarms.create(handler10_20, (void*)20);
  bool resched = Alarms.set(an1,10);
  ASSERT(!resched);
  ASSERT(Alarms.getUsed()==1);
  heapcheck(1);
  ASSERT(Alarms.canAdd());
  ASSERT(Alarms.canRemove());

  int an2 = Alarms.create(handler5_6, (void*)6);
  bool resched2 = Alarms.set(an2,5);
  ASSERT(!resched2);
  heapcheck(1);
  ASSERT(Alarms.getUsed()==2);

  timer_1msec_counter = 3;
  ASSERT(!Alarms.runDueEvents(99999999));
  ASSERT(Alarms.getUsed()==2);

  timer_1msec_counter = 4;
  ASSERT(!Alarms.runDueEvents(99999999));
  ASSERT(Alarms.getUsed()==2);

  timer_1msec_counter = 5;
  ASSERT(Alarms.runDueEvents(99999999));
  ASSERT(Alarms.getUsed()==1);

  timer_1msec_counter = 6;
  ASSERT(!Alarms.runDueEvents(99999999));
  ASSERT(Alarms.getUsed()==1);

  timer_1msec_counter = 6;
  ASSERT(!Alarms.runDueEvents(99999999));
  ASSERT(Alarms.getUsed()==1);

  timer_1msec_counter = 20;
  ASSERT(Alarms.runDueEvents(99999999));
  ASSERT(Alarms.getUsed()==0);

  heapcheck(1);

  runRandoms();
}

void randomHandler(u32 when, void * arg) {
  ASSERT(when==(u32)arg);
}
void runRandoms() {
  srandom(17);
  int added = 0;
  int full = 0;
  int empty = 0;
  int ran = 0;
  int cancelled = 0;
  int an = Alarms.create(randomHandler, 0);
  const int OPS = 2000000;
  for (int i = 0; i < OPS; ++i) {
    switch (random()%7) {
    case 0:
    case 1:
    case 2:
      if (Alarms.canAdd()) {
        u32 when = random()%1000000;
        Alarms.update(an,randomHandler,(void*)when);
        if (!Alarms.set(an,when))
          ++added;
        heapcheck(1);
      } else ++full;
      break;
    case 3:
    case 4:
      if (Alarms.canRemove()) {
        int when = Alarms.getTopTime();
        timer_1msec_counter = when;
        int thisRun = Alarms.runDueEvents(99999999);
        ASSERT(thisRun);
        ran += thisRun;
        heapcheck(1);
      } else ++empty;
      break;
    case 5:
      {
        u32 when = random()%1000000;
        int toRun = random()%20;
        timer_1msec_counter = when;
        int fired = Alarms.runDueEvents(toRun);
        ASSERT(fired <= toRun);
        ran += fired;
        heapcheck(1);
      }
      break;
    case 6:
      {
        int used = Alarms.getUsed();
        if (used > 0) {
          int slot = 1+random()%used;
          int eltIdx = Alarms.slots[slot];
          Alarms.cancel(eltIdx);
          ++cancelled;
        } else ++empty;
        heapcheck(1);
      }
      break;
    }
  }
  ASSERT(added == ran + cancelled + Alarms.getUsed());
}

int main() {
  runTests();
  return 0;
}
#endif
