#include <math.h>
#include "ascon.h"
#include "devser.h"

typedef struct DevAction {
  struct DevAction *next;
  void *data;
  DevActionHandler *hdl;
  DevPrio prio;
  DevKillActionData *kill;
  double interval; /* -1 for a queued action */
  double timeDue; /* 0 for a queued action */
} DevAction;

struct DevSer {
  Ascon *ascon;             /* connection */
  DevAction *current;
  int killCurrent;
  DevAction *actions;       /* the action queue */
  DevAction *toKill;        /* list of actions to be killed */
  int steps;
};

static char *devPrio[NumberOfPRIO] = {
  "null", "slow", "read", "progress", "write", "halt", "start"
};

char *DevPrio2Text(DevPrio prio)
{
  if (prio <= 0 || prio >= NumberOfPRIO) {
    prio = NullPRIO;
  }
  return devPrio[prio];
}

DevPrio DevText2Prio(char *text)
{
  DevPrio prio;
  for (prio = 0; prio < NumberOfPRIO; prio++) {
    if (strcasecmp(text, devPrio[prio]) == 0)
      return prio;
  }
  return NullPRIO;
}

static void DevFreeActionList(DevAction * actions)
{
  DevAction *victim;
  while (actions != NULL) {
    victim = actions;
    actions = victim->next;
    assert(victim->data != NULL);
    if (victim->kill != NULL)
      victim->kill(victim->data);
    victim->data=NULL;
    free(victim);
  }
}

static double nextTime(double last,
                       double lastInterval,
                       double now,
                       double interval) {
  double base;
  /* nextTime gives the next possible time according to the
   * following rules:
   * (1) result > now
   * (2) result is a multiple of interval
   * (3) result > last + min(lastInterval,interval) * 0.99
   */
  if (interval > lastInterval) {
    base = last + lastInterval * 0.99;
  } else {
    base = last + interval * 0.99;
  }
  if (now > base) {
    base = now;
  }
  return (floor(base / interval) + 1) * interval;
}

static DevAction *DevNextAction(DevSer * devser)
{
/* the action queue is primarily ordered by priority (high prioirty first),
 * within one priority by usage (last used action at end)
 */
  DevPrio prio;
  double now, next;
  DevAction *action, **ptr2prev;

  devser->current = NULL;
  now = DoubleTime();
  for (ptr2prev = &devser->actions, action = devser->actions;
       action != NULL;
       ptr2prev = &action->next, action = action->next) {
    if (now >= action->timeDue) {
      /* remove action from queue */
      *ptr2prev = action->next;
      devser->current = action;
      if (action->interval >= 0) {
        /* this is a scheduled action, to be preserved after use */
        devser->killCurrent = 0;
        if (action->interval == 0) {
          action->timeDue = now;
        } else {
          /* increase timeDue according to interval */
          action->timeDue = nextTime(action->timeDue, action->interval,
            now, action->interval);
        }
        prio = action->prio;
        
        /* insert devser->current before the next lower priority */
        for (action = action->next; /* start after this */
             action != NULL && action->prio >= prio;
             ptr2prev = &action->next, action = action->next);

        /* action is now NULL or the next action with lower priority */
        *ptr2prev = devser->current;
        devser->current->next = action;
      } else {
        /* this is a queued action, to be killed after use */
        devser->killCurrent = 1;
      }
      return devser->current;
    }
    if (action->prio == StartPRIO) {
      /* if a poll with StartPRIO is scheduled, block all other actions */
      return NULL;
    }
  }
  return NULL;
}

static int DevQueueTask(void *ds)
{
  DevSer *devser = ds;
  AsconStatus status;
  DevAction *action;
  char *sendData;
  char *replyData = NULL;
  
  if (devser->steps == 0)
    return 1;
  
  /* deferred deallocation of removed actions */
  DevFreeActionList(devser->toKill);
  devser->toKill = NULL;
  
  action = devser->current;
  if (action == NULL) {
    action = DevNextAction(devser);
  }

  while (action != NULL) {
    status = AsconTask(devser->ascon);
    if (status == AsconFailure) {
      replyData = AsconGetError(devser->ascon);
    } else if (status == AsconReady) {
      replyData = AsconRead(devser->ascon);
    } else {
      return 1;
    }
    if (devser->steps > 0) {    /* debugging mode */
      devser->steps--;
    }
    sendData = action->hdl(action->data, replyData, (status == AsconFailure));
    if (sendData != NULL) {
      AsconWrite(devser->ascon, sendData, 0);
      return 1;
    }
    if (devser->killCurrent) {
      if (action->kill != NULL)
        action->kill(action->data);
      devser->killCurrent = 0;
      free(action);
      devser->current = NULL;
    }
    action = DevNextAction(devser);
  }
  return 1;
}

DevSer *DevMake(SConnection * con, int argc, char *argv[])
{
  DevSer *devser = NULL;
  Ascon *ascon = NULL;

  ascon = AsconMake(con, argc, argv);
  if (!ascon) {
    return NULL;
  }
  devser = calloc(1, sizeof(*devser));
  assert(devser);
  devser->ascon = ascon;
  devser->current = NULL;
  devser->killCurrent = 0;
  devser->actions = NULL;
  devser->toKill = NULL;
  devser->steps = -1;           /* no debugging by default */
  TaskRegister(pServ->pTasker, DevQueueTask, NULL, NULL, devser, 0);
  return devser;
}

void DevDebugMode(DevSer * devser, int steps)
{
  devser->steps = steps;
}

void DevKill(DevSer * devser)
{
  if (devser->ascon) {
    AsconKill(devser->ascon);
  }
  DevFreeActionList(devser->actions);
  DevFreeActionList(devser->toKill);
  if (devser->killCurrent) {
    if (devser->current->kill != NULL) {
      devser->current->kill(devser->current);
    }
    devser->killCurrent = 0;
    free(devser->current);
  }
  TaskRemove(pServ->pTasker, DevQueueTask, devser);
  free(devser);
}

void DevDisconnect(DevSer * devser)
{
  if (devser->ascon) {
    AsconDisconnect(devser->ascon);
  }
}

int DevUnschedule(DevSer * devser, void *callData,
                  DevActionHandler * hdl, DevActionMatch * matchFunc)
{
  DevAction **ptr2prev = NULL;
  DevAction *action = NULL;
  int cnt = 0;

  /* scan through the queue */
  for (ptr2prev = &devser->actions, action = devser->actions;
       action != NULL;
       action = action->next) {
    if (action->hdl == hdl && matchFunc(callData, action->data)) {
      if (action == devser->current) {
        devser->current = NULL;
        devser->killCurrent = 0;
      }
      cnt++;
      /* remove from list */
      *ptr2prev = action->next;
      /* add to kill list */
      action->next = devser->toKill;
      devser->toKill = action;
    } else {
      ptr2prev = &action->next;
    }
  }
  return cnt;
}

int DevSchedule(DevSer * devser, void *actionData,
                DevPrio prio, double interval,
                DevActionHandler * hdl, DevActionMatch * matchFunc,
                DevKillActionData * killFunc)
{
  DevAction *action;
  DevAction *foundAction = NULL;
  DevAction **ptr2prev;
  DevAction **ptr2insertPos = NULL;
  int ret;
  
  if (prio <= NullPRIO) {
    prio = NullPRIO + 1;
  }
  if (prio >= NumberOfPRIO) {
    prio = NumberOfPRIO - 1;
  }
  
  for (ptr2prev = &devser->actions, action = devser->actions;
       action != NULL;
       ptr2prev = &action->next, action = action->next) {
    if (action->prio < prio && ptr2insertPos == NULL) {
      ptr2insertPos = ptr2prev;
    }
    /* check if it is the same action (only once) */
    if (action->hdl == hdl
        && action->kill == killFunc
        && (interval < 0) == (action->interval < 0)
        && foundAction == NULL
        && matchFunc(actionData, action->data)) {
      if (prio == action->prio && interval < 0) {
        /* do not move an action with equal prio */
        killFunc(actionData);
        return 0;
      }
      /* remove action from list */
      *ptr2prev = action->next;
      foundAction = action;
    }
  }
  
  /* create if needed */
  if (foundAction != NULL) {
    action = foundAction;
    ret = 0;
    killFunc(actionData);
  } else {
    action = calloc(1, sizeof(*action));
    assert(action);
    action->data = actionData;
    action->hdl = hdl;
    action->kill = killFunc;
    action->timeDue = 0;
    ret = 1;
  }
  
  action->prio = prio;
  /* insert into queue */
  if (ptr2insertPos == NULL) {
    ptr2insertPos = ptr2prev;
  }
  action->next = *ptr2insertPos;
  *ptr2insertPos = action;
  
  if (interval < 0) { /* case "queued" */
    action->interval = -1.0;
  } else { /* case "scheduled" */
    if (action->timeDue == 0) { /* not yet scheduled: do it immediately */
      action->timeDue = DoubleTime();
    } else { /* calculate next time */
      action->timeDue = nextTime(action->timeDue, action->interval,
        DoubleTime(), interval);
    }
    action->interval = interval;
  }
  return ret;
}

int DevQueue(DevSer * devser, void *actionData, DevPrio prio,
             DevActionHandler * hdl, DevActionMatch * matchFunc,
             DevKillActionData * killFunc)
{
  return DevSchedule(devser, actionData, prio, -1.0, hdl, matchFunc, killFunc);
}

int DevRemoveAction(DevSer * devser, void *actionData)
{
  DevAction **ptr2prev = NULL;
  DevAction *action = NULL; 
  int cnt = 0;
  
  
  /* Remove current action, if matched. If a reply is pending, the next
     action will get the reply. But as in the inital state no reply is
     expected, this should not harm. */
  action = devser->current;
  if (action != NULL && actionData == action->data) {
    if (devser->killCurrent) {
      if (action->kill != NULL)
        action->kill(action->data);
      devser->killCurrent = 0;
      free(action);
    }
    devser->current = NULL;
  }
  /* remove from queue */
  ptr2prev = &devser->actions;
  for (action = devser->actions; action != NULL; action = *ptr2prev) {
    if (actionData == action->data) {
      cnt++;
      /* remove from list */
      *ptr2prev = action->next;
      if (action->kill != NULL)
        action->kill(action->data);
      free(action);
    } else {
      ptr2prev = &action->next;
    }
  }
  return cnt++;
}

int DevIsPending(DevSer * devser, void *callData,
                 DevActionHandler * hdl, DevActionMatch * matchFunc)
{
  DevAction *action = devser->current;

  if (action) {
    if (action->hdl == hdl && matchFunc(callData, action->data)) {
      return 1;
    }
  }
  return 0;
}