/* ----------------------------------------------------------------------------
fsm.c

a finite state machine within sics

M. Zolliker, Aug 2004
---------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include "fsm.h"
#include "statistics.h"

#define MAXSTACK 8

typedef struct {
  long pc;
  FsmFunc func;
} StackItem;

struct Fsm {
  void *obj;
  FsmHandler handler;
  long pc;
  FsmFunc func;
  time_t till;
  int pause;
  int sp;
  StackItem stack[MAXSTACK];
  Statistics *stat;
};


static Fsm *fsm = NULL;
static FsmFunc callFunc = NULL;

void FsmWait(long delay)
{
  assert(fsm);
  fsm->till = time(NULL) + delay;
}

void FsmSpeed(Fsm * task)
{
  assert(task);
  if (task->till != 0)
    task->till = time(NULL);
}

long FsmPc(void) {
  if (fsm) {
    return fsm->pc;
  }
  return -1;
}

int FsmTaskHandler(Fsm * task)
{
  Statistics *old;

  old = StatisticsBegin(task->stat);
  if (task->pause) {
    task->handler(task->obj);
    StatisticsEnd(old);
    return 1;
  }
  if (task->pc >= 0) {          /* task->pc < 0 means stop current function */
    if (task->till != 0) {
      if (time(NULL) < task->till) {
        StatisticsEnd(old);
        return 1;               /* wait */
      }
      task->till = 0;
    }
    if (task->handler(task->obj) == 0) {
      StatisticsEnd(old);
      return 1;                 /* wait for answer */
    }
    fsm = task;
    task->pc = task->func(task->pc, task->obj);
    if (task->pc > 0)
      task->pc++; /* go to next line (case must be one line after return) */
    while (callFunc) {
      assert(fsm->sp < MAXSTACK);
      fsm->stack[fsm->sp].pc = task->pc;
      fsm->stack[fsm->sp].func = fsm->func;
      fsm->sp++;
      fsm->pc = 0;
      fsm->func = callFunc;
      callFunc = NULL;
      task->pc = fsm->func(fsm->pc, fsm->obj);
      if (task->pc > 0)
        task->pc++; /* go to next line (case must be one line after return) */
    }
    fsm = NULL;
  }
  StatisticsEnd(old);
  if (task->pc <= 0) {
    task->pc = 0;
    if (task->sp == 0) {
      return (task->obj != NULL);       /* finish task only when explicitely stopped */
    }
    if (task->sp > 0) {
      task->sp--;
      task->pc = task->stack[task->sp].pc;
      task->func = task->stack[task->sp].func;
    }
  }
  return 1;
}

void FsmKill(void *data)
{
  free(data);
}

void FsmRestartTask(Fsm * task, FsmFunc func)
{
  task->pc = 0;
  task->func = func;
  task->sp = 0;
  task->pause = 0;
  task->till = 0;
}

Fsm *FsmStartTask(void *obj, FsmHandler handler, FsmFunc func, char *name)
{
  Fsm *task;
  char fsmName[80];

  task = malloc(sizeof *task);
  task->obj = obj;
  task->handler = handler;
  snprintf(fsmName, sizeof fsmName, "fsm %s", name);
  task->stat = StatisticsNew(fsmName);
  FsmRestartTask(task, func);
  return task;
}

int FsmStop(Fsm * task, FsmFunc func)
{
  int i;

  if (task == NULL)
    task = fsm;
  if (func == NULL)
    return 0;
  assert(task);
  for (i = 0; i < task->sp; i++) {
    if (func == task->stack[i].func) {
      break;
    }
  }
  if (i == task->sp) {          /* not found on stack */
    if (func != task->func)
      return 0;                /* is also not running function */\
    task->sp = 0;
    task->func = task->stack[0].func; /* pretty unsure about this */
  } else {
    task->sp = i;               /* unwind stack to level i */
  }
  task->pc = -1;                /* leave function */
  return 1;
}

void FsmStopTask(Fsm * task)
{
  assert(task);
  task->sp = 0;
  task->pc = -1;
  task->obj = NULL;
}

void FsmPause(Fsm * task, int pause)
{
  task->pause = pause;
}

void FsmCall(FsmFunc func)
{
  assert(fsm);
  callFunc = func;
}