/*------------------------------------------------------------------------

                                T A S K E R

  Implementation of a portable task switcher.

  Mark Koennecke, September 1997

  copyleft, 1997.

  NO WARRANTIES OF ANY KIND WHATSOEVER TAKEN BY ME OR MY EMPLOYER.
  YOU ARE AT YOUR OWN!

  Reworked to have a task name, stop by task name and better listing.

  Mark Koennecke, December 2012

  Reworked to implement priorities and message queues

  Douglas Clowes, July 2015
----------------------------------------------------------------------------*/
#include <stdlib.h>
#include <stdarg.h>
#include <assert.h>
#include <time.h>
#include <sys/time.h>
#include <stdio.h>
#include "fortify.h"
#include "task.h"
#define READY 1
#define WAITING 2
#define YIELDING 3
#define DERFUNCT 4

#define IDUNDEFINED 0L
#define TASKMAGIC 777111999

typedef unsigned long long LoopCounter;
/*--------------------------------------------------------------------------*/
struct  __TaskTaskID {
  long lID;
};

struct __TaskGroupID {
  long lID;
};

typedef struct{
  pTaskMan tasker;
  TaskGroupID groupID;
} TaskGroupData, *pTaskGroupData;

TaskTaskID TaskUnknownTaskID = 0;
TaskGroupID TaskUnknownGroupID = 0;

TaskTaskID TaskBadTaskID = -1;
TaskGroupID TaskBadGroupID = -1;
/*--------------------------------------------------------------------------*/
typedef struct __TaskHead {
  long magic;
  TaskTaskID taskID;
  TaskGroupID groupID;
  int iStatus;
  int iPrioBase;
  TaskTaskID waitTarget;
  char *name;
  time_t start_time;
  double startRunTime;
  double lastRunTime;
  double nextRunTime;
  double nextRunPeriod;
  TaskFunc pRun;
  TaskMsgFunc pMsgRun;
  SignalFunc pSignal;
  void *pData;
  TaskKillFunc pKill;
  pTaskHead pNext;
  pTaskHead pPrevious;
  pTaskQueue pQueue;
  LoopCounter numRuns;
  LoopCounter numWaits;
  LoopCounter numYields;
  double startWallTime;
  double accumulatedWallTime;
  double startYieldTime;
  double accumulatedYieldTime;
  double startProcessorTime;
  double accumulatedProcessorTime;
  int isActive;
  int isLogged;
} TaskHead;

typedef struct __TaskMan {
  int iID;
  int iStop;
  pTaskHead pCurrent; /* Think trice before you interfere with this! */
  pTaskHead pHead;
  int stackPointer;
  TaskTaskID stackStack[20];
} TaskMan;

static TaskLogFunc TaskLogWrite = NULL;
static eTaskLogLevel TaskLogLevel = eTaskLogNone;

static int LOGGING_AT(pTaskMan self, eTaskLogLevel level) {
  if (TaskLogWrite == NULL)
    return 0;
  if (TaskLogLevel == eTaskLogNone)
    return 0;
  if (level < TaskLogLevel)
    return 0;
  return 1;
}

static int TaskLogPrint(eTaskLogLevel lvl, const char *fmt, ...)
{
  va_list ap;
  char buf[256];
  int l;

  if (TaskLogWrite == NULL || TaskLogLevel == eTaskLogNone || lvl < TaskLogLevel) {
    return 0;
  }

  va_start(ap, fmt);
  l = vsnprintf(buf, sizeof buf, fmt, ap);
  va_end(ap);
  if (l <= 0)
    return l;
  if (l >= sizeof buf) {
    char *dyn;
    /* we have probably a C99 conforming snprintf and
       need a larger buffer
     */
    dyn = malloc(l + 1);
    if (dyn != NULL) {
      va_start(ap, fmt);
      vsnprintf(dyn, l + 1, fmt, ap);
      va_end(ap);
      TaskLogWrite(lvl, dyn);
      free(dyn);
      return l;
    }
  }
  TaskLogWrite(lvl, buf);
  return l;
}

void TaskLogStack(pTaskMan self, eTaskLogLevel typ);
/*---------------------------------------------------------------------------
The 7 below solves a subtle bug which occurs when a groupID in user code
has been initialized to 0 and starting fails. Then it seems as if this
group keeps running. As there will always be some task running at 0.
  ----------------------------------------------------------------------------*/
static long lIDMama = 7L;
#define  TASKERID 123399
/*---------------------------------------------------------------------------*/
static double DoubleTime(void)
{
  struct timeval now;
  /* the resolution of this function is usec, if the machine supports this
     and the mantissa of a double is 51 bits or more (31 for sec and 20 for mic$
   */
  gettimeofday(&now, NULL);
  return now.tv_sec + now.tv_usec / 1e6;
}

/*-------------------------------------------------------------------------*/
static double DoubleTimeCPU(void)
{
  struct timespec now;
  /* the resolution of this function is usec, if the machine supports this
     and the mantissa of a double is 51 bits or more (31 for sec and 20 for mic$
   */
  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &now);
  return now.tv_sec + now.tv_nsec / 1e9;
}

/*---------------------------------------------------------------------------*/
static pTaskHead MakeTaskHead(char *name, TaskFunc pTask, SignalFunc pSignal,
                              void *pData, TaskKillFunc pKill)
{
  pTaskHead pNew = NULL;
  TaskTaskID newID;

  pNew = (pTaskHead) malloc(sizeof(TaskHead));
  if (!pNew) {
    return NULL;
  }
  memset(pNew, 0, sizeof(TaskHead));

  pNew->magic = TASKMAGIC;
  pNew->name = strdup(name);
  pNew->start_time = time(NULL);
  pNew->startRunTime = DoubleTime();
  pNew->pRun = pTask;
  pNew->pSignal = pSignal;
  pNew->pData = pData;
  pNew->pKill = pKill;
  lIDMama++;
  if(lIDMama < 7){
    lIDMama = 7;
  }
  newID = lIDMama;
  pNew->taskID = newID;
  pNew->iStatus = READY;
  pNew->groupID = TaskUnknownGroupID;
  pNew->waitTarget = TaskUnknownTaskID;

  return pNew;
}

/*--------------------------------------------------------------------------*/
static void DeleteTaskHead(pTaskHead self)
{
  assert(self);
  void *pData;

  /* unlink first to prevent double handling when Kill calls Yield*/
  if (self->pPrevious != NULL) {
    self->pPrevious->pNext = self->pNext;
  }
  if (self->pNext != NULL) {
    self->pNext->pPrevious = self->pPrevious;
  }


  if (self->pKill) {
    if (self->pData) {
      pData = self->pData;
      self->pData = NULL;
      self->pKill(pData);
    }
  }
  if(self->name != NULL){
    free(self->name);
  }

  memset(self,0,sizeof(TaskHead));
  free(self);
}

/*--------------------------------------------------------------------------*/
static int DummyTask(void *pData)
{
  return 1;
}

/*--------------------------------------------------------------------------*/
static double makeTaskActive(pTaskHead self)
{
  double now = DoubleTime();

  self->isActive = 1;
  self->startWallTime = now;
  self->startProcessorTime = DoubleTimeCPU();
  return now;
}

/*--------------------------------------------------------------------------*/
static double makeTaskIdle(pTaskHead self)
{
  double now = DoubleTime();

  self->accumulatedWallTime += now - self->startWallTime;
  self->accumulatedProcessorTime +=  DoubleTimeCPU() - self->startProcessorTime;
  self->isActive = 0;
  return now;
}

/*--------------------------------------------------------------------------*/
static int makeTaskRun(pTaskMan self)
{
  int iRet;
  LoopCounter numWaits;
  LoopCounter numYields;
  double start, elapsed;
  pTaskHead pThis = self->pCurrent;
  start = makeTaskActive(pThis);
  numWaits = pThis->numWaits;
  numYields = pThis->numYields;
  if (pThis->pRun)
    iRet = pThis->pRun(pThis->pData);
  else if (pThis->pMsgRun) {
    pTaskMessage pMsg = TaskQueueRecvMine(self);
    if (pMsg) {
      iRet = pThis->pMsgRun(pThis->pData, pMsg);
      TaskMessageFree(pMsg);
    } else {
      /* TODO: should not happen */
    }
  } else {
    /* TODO: this should not happen */
  }
  pThis->isLogged = 0; /* reset this after each run */
  pThis->lastRunTime = start;
  pThis->numRuns++;
  elapsed = makeTaskIdle(pThis) - start;
  if (LOGGING_AT(self, eTaskLogDebug)) {
    if (pThis->numWaits != numWaits)
      TaskLogPrint(eTaskLogDebug, "TaskWaited: %s waited %lld times in %fS",
          pThis->name,
          pThis->numWaits - numWaits,
          elapsed);
    if (pThis->numYields != numYields)
      TaskLogPrint(eTaskLogDebug, "TaskYielded: %s yielded %lld times in %fS",
          pThis->name,
          pThis->numYields - numYields,
          elapsed);
    if (pThis->numWaits != numWaits || pThis->numYields != numYields)
      if (self->stackPointer > 0)
        TaskLogStack(self, eTaskLogDebug);
  }
  return iRet;
}

/*--------------------------------------------------------------------------*/
int TaskerInit(pTaskMan * self)
{
  pTaskMan pNew = NULL;
  pTaskHead pDummyTask = NULL;

  *self = NULL;
  /* a new Task manager */
  pNew = (pTaskMan) malloc(sizeof(TaskMan));
  if (!pNew) {
    return 0;
  }
  memset(pNew, 0, sizeof(TaskMan));
  pNew->iID = TASKERID;

  /* create a dummy task as start point */
  pDummyTask = MakeTaskHead("init",DummyTask, NULL, NULL, NULL);
  if (!pDummyTask) {
    free(pNew);
    return 0;
  }

  /* link */
  pDummyTask->pNext = NULL;
  pDummyTask->pPrevious = NULL;

  pNew->pCurrent = pDummyTask;
  pNew->pHead = pDummyTask;
  *self = pNew;
  return 1;
}

/*---------------------------------------------------------------------------*/
int TaskerDelete(pTaskMan * pData)
{
  pTaskMan self = *pData;
  pTaskHead pCurrent, pKill;

  assert(self);
  assert(self->iID == TASKERID);

  pCurrent = self->pHead;
  while (pCurrent != NULL) {
    pKill = pCurrent;
    pCurrent = pCurrent->pNext;
    DeleteTaskHead(pKill);
  }
  free(self);
  *pData = NULL;
  return 1;
}
/*---------------------------------------------------------------------------*/
static pTaskHead taskConstructor(pTaskMan self, char *name, TaskFunc pTask, SignalFunc pSignal,
                  TaskKillFunc pKill, void *pData, int iPriority)
{
  pTaskHead pNew = NULL;

  assert(self);
  assert(self->iID == TASKERID);

  pNew = MakeTaskHead(name, pTask, pSignal, pData, pKill);
  if (!pNew) {
    return NULL;
  }
  pNew->iPrioBase = iPriority;

  /* link it in */
  if (self->pCurrent->pNext) {
    self->pCurrent->pNext->pPrevious = pNew;
  }
  pNew->pPrevious = self->pCurrent;
  pNew->pNext = self->pCurrent->pNext;
  self->pCurrent->pNext = pNew;

  if (LOGGING_AT(self, eTaskLogDebug)) {
    TaskLogPrint(eTaskLogDebug, "TaskStart: %s (ID = %ld, Prio = %d)",
        pNew->name, pNew->taskID, pNew->iPrioBase);
  }
  return pNew;
}
/*----------------------- temporary for backwards compatability -------------*/
TaskTaskID TaskRegister(pTaskMan self,TaskFunc pTask, SignalFunc pSignal,
                  TaskKillFunc pKill, void *pData, int iPriority)
{
  pTaskHead pNew;
  assert(pTask);
  pNew = taskConstructor(self, "Unknown", pTask, pSignal, pKill, pData, iPriority);
  return pNew ? pNew->taskID : TaskBadTaskID;
}
/*---------------------------------------------------------------------------*/
TaskTaskID TaskRegisterN(pTaskMan self, char *name, TaskFunc pTask, SignalFunc pSignal,
                  TaskKillFunc pKill, void *pData, int iPriority)
{
  pTaskHead pNew;
  assert(pTask);
  pNew = taskConstructor(self, name, pTask, pSignal, pKill, pData, iPriority);
  return pNew ? pNew->taskID : TaskBadTaskID;
}
/*---------------------------------------------------------------------------*/
TaskTaskID TaskRegisterQ(pTaskMan self, char *name, TaskMsgFunc pTask, SignalFunc pSignal,
                  TaskKillFunc pKill, void *pData, int iPriority)
{
  pTaskHead pNew;
  assert(pTask);
  pNew = taskConstructor(self, name, NULL, pSignal, pKill, pData, iPriority);
  if (pNew) {
    pNew->pMsgRun = pTask;
    pNew->pQueue = TaskQueueAlloc();
  }
  return pNew ? pNew->taskID : TaskBadTaskID;
}
/*---------------------------------------------------------------------------*/
TaskTaskID TaskRegisterD(pTaskMan self, char *name, TaskFunc pTask, SignalFunc pSignal,
                  TaskKillFunc pKill, void *pData, int iPriority, double delay)
{
  pTaskHead pNew;
  assert(pTask);
  pNew = taskConstructor(self, name, pTask, pSignal, pKill, pData, iPriority);
  if (pNew) {
    pNew->nextRunTime = DoubleTime() + delay;
  }
  return pNew ? pNew->taskID : TaskBadTaskID;
}
/*---------------------------------------------------------------------------*/
TaskTaskID TaskRegisterP(pTaskMan self, char *name, TaskFunc pTask, SignalFunc pSignal,
                  TaskKillFunc pKill, void *pData, int iPriority,
                  double delay, double period)
{
  pTaskHead pNew;
  assert(pTask);
  pNew = taskConstructor(self, name, pTask, pSignal, pKill, pData, iPriority);
  if (pNew) {
    pNew->nextRunTime = DoubleTime() + delay;
    pNew->nextRunPeriod = period;
  }
  return pNew ? pNew->taskID : TaskBadTaskID;
}
/*-------------------------------------------------------------------------*/
static void IncrTaskPointer(pTaskMan self)
{
  self->pCurrent = self->pCurrent->pNext;
  if (self->pCurrent == NULL) {
    self->pCurrent = self->pHead;
  }
}

/*-------------------------------------------------------------------------*/
static int TaskExist(pTaskMan self, TaskTaskID taskID)
{
  if (taskID == TaskBadTaskID || taskID == TaskUnknownTaskID) {
    return 0;
  } else {
    pTaskHead pCur = self->pHead;
    while (pCur != NULL) {
      if (pCur->taskID == taskID) {
        return 1;
      }
      pCur = pCur->pNext;
    }
    return 0;
  }
}

/*--------------------------------------------------------------------------*/
static void TaskPush(pTaskMan self)
{
  makeTaskIdle(self->pCurrent);
  self->stackStack[self->stackPointer] = self->pCurrent->taskID;
  self->stackPointer++;
}
/*--------------------------------------------------------------------------*/
static void TaskPop(pTaskMan self)
{
  makeTaskActive(self->pCurrent);
  self->stackPointer--;
}
/*--------------------------------------------------------------------------*/
static void TaskPrune(pTaskMan self)
{
  pTaskHead pThis;
  pThis = self->pHead; /* start this at the dummy */
  while (pThis->pNext) {
    if (pThis->pNext->iStatus == DERFUNCT) {
      pTaskHead pThat = pThis->pNext;
      if (LOGGING_AT(self, eTaskLogDebug))
        TaskLogPrint(eTaskLogDebug,
            "TaskEnd: %s (ID=%ld,Loops=%lld,Waits=%lld,Yields=%lld,CPU=%f,Wall=%f,Yield=%f,Up=%f)",
            pThat->name,
            pThat->taskID,
            pThat->numRuns,
            pThat->numWaits,
            pThat->numYields,
            pThat->accumulatedProcessorTime,
            pThat->accumulatedWallTime,
            pThat->accumulatedYieldTime,
            (double) (DoubleTime() - pThat->startRunTime)
            );
      DeleteTaskHead(pThat); /* unlink and delete it */
    }
    pThis = pThis->pNext; /* Note: not the same as pThat above */
  }
}
/*--------------------------------------------------------------------------*/
static void TaskLoop(pTaskMan self)
{
  int iRet;
  pTaskHead pThis; /* The current task on entry, or dummy */
  pTaskHead pThat; /* The task we are thinking of running next */
  pThis = self->pCurrent;
  while (self->iStop == 0) {
    pThat = self->pCurrent;
    if (pThat->iStatus != READY) {
      /* Not ready: do nothing */
    } else if (pThat->pQueue && TaskQueueCount(pThat->pQueue) == 0) {
      /* Have Queue, but is empty: do nothing */
    } else if (pThat->nextRunTime > 0.0 && DoubleTime() < pThat->nextRunTime) {
      /* Have due time but not there yet: do nothing */
    } else if (pThis->iPrioBase <= pThat->iPrioBase) {
        if (pThat->nextRunTime > 0.0) {
          /* Do this now so the task can override it during the run */
          if (pThat->nextRunPeriod > 0) {
            double now = DoubleTime();
            pThat->nextRunTime += pThat->nextRunPeriod;
            if (now > pThat->nextRunTime)
              pThat->nextRunTime = now + pThat->nextRunPeriod;
          } else {
            pThat->nextRunTime = 0.0;
          }
        }
        iRet = makeTaskRun(self);
        if (iRet == 0) {
          pThat->iStatus = DERFUNCT;
        }
    }
    IncrTaskPointer(self);
    if (self->pCurrent == pThis)
      break;
  }
  TaskPrune(self);
}
/*--------------------------------------------------------------------------*/
int TaskSchedule(pTaskMan self)
{
  int iRet;

  assert(self);
  assert(self->iID == TASKERID);

  /* forever, until stop is called somehow */
  while (self->iStop == 0)
    TaskLoop(self);
  return 1;
}

/*----------------------------------------------------------------*/
int TaskYield(pTaskMan self)
{
  int iRet;
  double deltaTime;
  pTaskHead pTemp, pThis;
  extern void generate_stack_trace(int full, int dump);

  assert(self);
  assert(self->iID == TASKERID);

  /* Cycle until back at ourselves  */
  pThis = self->pCurrent;
  pThis->iStatus = WAITING;
  TaskPush(self);
  self->pCurrent->numYields++;
  if (LOGGING_AT(self, eTaskLogWarning)) {
    if (pThis->iPrioBase > TASK_PRIO_LOW) {
      if (!pThis->isLogged) {
        pThis->isLogged = 1;
        TaskLogPrint(eTaskLogWarning, "TaskYield: Unexpected Yield (not TASK_PRIO_LOW)");
        TaskLogStack(self, eTaskLogWarning);
        generate_stack_trace(1, 0);
      }
    }
  }
  pThis->startYieldTime = DoubleTime();
  TaskLoop(self);
  deltaTime = DoubleTime() - pThis->startYieldTime;
  if (pThis->iPrioBase > TASK_PRIO_LOW && deltaTime > 0.250) {
    if (LOGGING_AT(self, eTaskLogDebug)) {
      TaskLogPrint(eTaskLogWarning, "TaskYield: Excessive Yield Time %fS in %s",
          deltaTime, pThis->name);
    }
  }
  pThis->accumulatedYieldTime += deltaTime;
  pThis->iStatus = READY;
  TaskPop(self);
  return 1;
}

/*-----------------------------------------------------------------*/
int TaskWait(pTaskMan self, TaskTaskID taskID)
{
  int iRet;
  pTaskHead pTemp, pThis;
  extern void generate_stack_trace(int full, int dump);

  assert(self);
  assert(self->iID == TASKERID);

  /* Cycle until taskID is killed. Stop is obeyed as well */
  pThis = self->pCurrent;
  pThis->iStatus = WAITING;
  pThis->waitTarget = taskID;
  TaskPush(self);
  self->pCurrent->numWaits++;
  if (LOGGING_AT(self, eTaskLogWarning)) {
    if (pThis->iPrioBase > TASK_PRIO_LOW) {
      if (!pThis->isLogged) {
        pThis->isLogged = 1;
        TaskLogPrint(eTaskLogWarning, "TaskWait: Unexpected Wait (not TASK_PRIO_LOW)");
        TaskLogStack(self, eTaskLogWarning);
        generate_stack_trace(1, 0);
      }
    }
  }
  pTemp = TaskIteratorByID(self, taskID);
  if (pTemp && pTemp->iPrioBase < pThis->iPrioBase) {
    /* Inverted priority */
    TaskLogPrint(eTaskLogError, "TaskWait: Bad Wait: Priority Inverted waiter(%s:%d) > target(%s:%d)",
        pThis->name, pThis->iPrioBase, pTemp->name, pTemp->iPrioBase);
    pTemp->iPrioBase = pThis->iPrioBase;
  }
  while (self->iStop == 0 && TaskExist(self, taskID))
    TaskLoop(self);
  pThis->waitTarget = TaskUnknownTaskID;
  pThis->iStatus = READY;
  TaskPop(self);
  return 1;
}

/*---------------------------------------------------------------------------*/
int TaskSignal(pTaskMan self, int iSignal, void *pSigData)
{
  pTaskHead pTemp, pEnd;
  int myStatus = self->pCurrent->iStatus;

  assert(self);
  assert(self->iID == TASKERID);

  /* Do one cycle until we are at the caller, then return to him */
  pEnd = self->pCurrent;
  pEnd->iStatus = WAITING;
  IncrTaskPointer(self);
  while (self->pCurrent != pEnd) {
    if (self->pCurrent->pSignal) {
      self->pCurrent->pSignal(self->pCurrent->pData, iSignal, pSigData);
    }
    IncrTaskPointer(self);
  }
  /* finally, tell me about the thingie as well */
  pEnd->iStatus = myStatus;
  if (pEnd->pSignal) {
    pEnd->pSignal(pEnd->pData, iSignal, pSigData);
  }

  return 1;
}

/*--------------------------------------------------------------------------*/
int TaskStop(pTaskMan self)
{
  assert(self);
  assert(self->iID == TASKERID);

  self->iStop = 1;
  return 1;
}

/*--------------------------------------------------------------------------*/
int TaskContinue(pTaskMan self)
{
  assert(self);
  assert(self->iID == TASKERID);

  self->iStop = 0;
  return 1;
}

/*--------------------------------------------------------------------------*/
void TaskRemove(pTaskMan self, TaskFunc pTaskRun, void *pData)
{
  int iRet;
  pTaskHead pCurrent, pNext;

  if (self == NULL) return;
  assert(self->iID == TASKERID);

  pNext = self->pHead->pNext;   /* skip dummy task */
  while (pNext != NULL) {
    pCurrent = pNext;
    pNext = pCurrent->pNext;
    if (pCurrent->pRun == pTaskRun && pCurrent->pData == pData) {
      if(pCurrent == self->pCurrent){
        /* cannot kill myself */
        return;
      }
      /* unlink */
      if (pCurrent->pPrevious != NULL) {
        pCurrent->pPrevious->pNext = pCurrent->pNext;
      }
      if (pCurrent->pNext != NULL) {
        pCurrent->pNext->pPrevious = pCurrent->pPrevious;
      }
      if(pCurrent->name != NULL){
        free(pCurrent->name);
      }
      free(pCurrent);
    }
  }
  return;
}
/*-----------------------------------------------------------------------------*/
int StopTask(pTaskMan self, char *name)
{
  int iRet;
  pTaskHead pCurrent, pNext;

  if (self == NULL) return 0;
  assert(self->iID == TASKERID);

  pNext = self->pHead->pNext;   /* skip dummy task */
  while (pNext != NULL) {
    pCurrent = pNext;
    pNext = pCurrent->pNext;
    if (strcmp(pCurrent->name,name) == 0) {
      if(self->pCurrent == pCurrent){
        /**
         * cannot kill myself
        */
        return 0;
      }
      /* unlink */
      if (pCurrent->pPrevious != NULL) {
        pCurrent->pPrevious->pNext = pCurrent->pNext;
      }
      if (pCurrent->pNext != NULL) {
        pCurrent->pNext->pPrevious = pCurrent->pPrevious;
      }
      if(pCurrent->name != NULL){
        free(pCurrent->name);
      }
      free(pCurrent);
    }
  }
  return 1;
}
/*-----------------------------------------------------------------------------*/
int isTaskRunning(pTaskMan self, char *name)
{
  int iRet;
  pTaskHead pCurrent, pNext;

  if (self == NULL) return 0;
  assert(self->iID == TASKERID);

  pNext = self->pHead->pNext;   /* skip dummy task */
  while (pNext != NULL) {
    pCurrent = pNext;
    pNext = pCurrent->pNext;
    if (strcmp(pCurrent->name,name) == 0) {
      return 1;
    }
  }
  return 0;
}
/*-----------------------------------------------------------------------------*/
int isTaskIDRunning(pTaskMan self, TaskTaskID taskID)
{
  int iRet;
  pTaskHead pCurrent, pNext;

  if (self == NULL) return 0;
  assert(self->iID == TASKERID);

  pNext = self->pHead->pNext;   /* skip dummy task */
  while (pNext != NULL) {
    pCurrent = pNext;
    pNext = pCurrent->pNext;
    if (pCurrent->taskID == taskID) {
      return 1;
    }
  }
  return 0;
}
/*-----------------------------------------------------------------------------*/
pTaskHead TaskIteratorStart(pTaskMan self)
{
  if (self == NULL) return NULL;
  assert(self->iID == TASKERID);

  return self->pHead->pNext; /* skip dummy task */
}
/*-----------------------------------------------------------------------------*/
pTaskHead TaskIteratorNext(pTaskHead it)
{
  if(it != NULL && it->magic == TASKMAGIC){
    return it->pNext;
  }
  return NULL;
}
/*-----------------------------------------------------------------------------*/
pTaskHead TaskIteratorCurrent(pTaskMan self)
{
  if (self == NULL) return NULL;
  assert(self->iID == TASKERID);

  return self->pCurrent;
}
/*-----------------------------------------------------------------------------*/
pTaskHead TaskIteratorByName(pTaskMan self, const char* name)
{
  pTaskHead it;
  if (self == NULL) return NULL;
  assert(self->iID == TASKERID);

  it = self->pHead->pNext; /* skip dummy task */
  while (it) {
    if (strcasecmp(it->name, name) == 0)
      break;
    it = it->pNext;
  }
  return it;
}
/*-----------------------------------------------------------------------------*/
pTaskHead TaskIteratorByID(pTaskMan self, TaskTaskID taskID)
{
  pTaskHead it;
  if (self == NULL) return NULL;
  assert(self->iID == TASKERID);

  it = self->pHead->pNext; /* skip dummy task */
  while (it) {
    if (it->taskID == taskID)
      break;
    it = it->pNext;
  }
  return it;
}
/*-----------------------------------------------------------------------------*/
char *TaskDescription(pTaskHead it)
{
  char *result;
  int length;
  const struct tm *tm;

  if(it == NULL){
    return NULL;
  }

  length = strlen(it->name) + 120;
  result = malloc(length*sizeof(char));
  if(result == NULL){
    return NULL;
  }
  memset(result,0,length*sizeof(char));
  strcpy(result,it->name);
  strcat(result,"|");

  length = strlen(result);
  tm = localtime((const time_t *)&it->start_time);
  strftime(result+length,100,"%F-%H-%M-%S",tm);
  length = strlen(result);
  snprintf(result+length,120-20,"|%ld", it->taskID);
  length = strlen(result);
  snprintf(result+length,120-40,"|%.0ld ", it->groupID);

  return result;

}
/*-----------------------------------------------------------------------------*/
char *TaskDetail(pTaskHead it)
{
  char *result;
  int length;
  const struct tm *tm;

  if(it == NULL){
    return NULL;
  }

  length = strlen(it->name) + 120;
  result = malloc(length*sizeof(char));
  if(result == NULL){
    return NULL;
  }
  memset(result,0,length*sizeof(char));
  strcpy(result,it->name);

  length = strlen(result);
  snprintf(result+length,120-20,"|%lld", it->numRuns);
  length = strlen(result);
  snprintf(result+length,120-20,"|%lld", it->numWaits);
  length = strlen(result);
  snprintf(result+length,120-20,"|%lld", it->numYields);
  length = strlen(result);
  snprintf(result+length,120-20,"|%.6f", it->accumulatedProcessorTime);
  length = strlen(result);
  snprintf(result+length,120-40,"|%.6f", it->accumulatedWallTime);
  length = strlen(result);
  snprintf(result+length,120-40,"|%.6f", it->accumulatedYieldTime);

  return result;

}
/*------------------------------------------------------------------------------*/
int TaskGetStack(pTaskMan self, pTaskHead it[])
{
  if (it != NULL) {
    int i;
    for (i = 0; i < self->stackPointer; ++i) {
      pTaskHead pTemp;
      it[i] = NULL;
      for (pTemp = self->pHead->pNext; pTemp; pTemp = pTemp->pNext) {
        if (pTemp->taskID == self->stackStack[i]) {
          it[i] = pTemp;
          break;
        }
      }
    }
  }
  return self->stackPointer;
}
/*------------------------------------------------------------------------------*/
void TaskLogStack(pTaskMan self, eTaskLogLevel typ)
{
  if (LOGGING_AT(self, typ)) {
    int i, count;
    pTaskHead it[20];
    count = TaskGetStack(self, it);
    for (i = 0; i < count; ++i) {
      TaskLogPrint(typ, "TaskStack[%02d]: %s", i, it[count - 1 - i]->name);
    }
  }
}
/*------------------------------------------------------------------------------*/
TaskTaskID GetTaskID(pTaskHead it)
{
  return it->taskID;
}
/*------------------------------------------------------------------------------*/
TaskGroupID GetGroupID(pTaskHead it)
{
  return it->groupID;
}
/*------------------------------------------------------------------------------*/
const char * GetTaskName(pTaskHead it)
{
  return (const char*)it->name;
}
/*------------------------------------------------------------------------------*/
const void *GetTaskData(pTaskHead it)
{
  if(it->magic == TASKMAGIC){
    return (const void*)it->pData;
  } else {
    return NULL;
  }
}
/*------------------------------------------------------------------------------*/
TaskGroupID GetTaskGroupID(pTaskMan self)
{
  TaskGroupID newGroupID;
  lIDMama++;
  if(lIDMama < 7){
    lIDMama = 7;
  }
  newGroupID = lIDMama;
  return newGroupID;
}
/*-------------------------------------------------------------------------------*/
void AddTaskToGroup(pTaskMan self, TaskTaskID taskID, TaskGroupID groupID)
{
  pTaskHead pCurrent, pNext;

  if (self == NULL) return;
  assert(self->iID == TASKERID);

  pNext = self->pHead->pNext;   /* skip dummy task */
  while (pNext != NULL) {
    pCurrent = pNext;
    pNext = pCurrent->pNext;
    if (pCurrent->taskID == taskID) {
      pCurrent->groupID = groupID;
      return;
    }
  }
}
/*---------------------------------------------------------------------------------
This simply checks if there are any more tasks with the desired groupID in the
list. If none, then all sub tasks have finished.
-----------------------------------------------------------------------------------*/
int isTaskGroupRunning(pTaskMan self, TaskGroupID groupID)
{
  pTaskHead pCurrent, pNext;

  if (self == NULL) return 0;
  if (groupID == TaskUnknownGroupID || groupID == TaskBadGroupID)
    return 0;

  assert(self->iID == TASKERID);

  pNext = self->pHead->pNext;   /* skip dummy task */
  while (pNext != NULL) {
    pCurrent = pNext;
    pNext = pCurrent->pNext;
    if (pCurrent->groupID == groupID) {
      return 1;
    }
  }
  return 0;
}
/*------------------------------------------------------------------------------*/
int TaskGroupTask(void *data)
{
  pTaskGroupData self = (pTaskGroupData)data;

  return isTaskGroupRunning(self->tasker,self->groupID);
}
/*-------------------------------------------------------------------------------*/
int TaskSignalGroup(pTaskMan self, int iSignal, void *pSigData, TaskGroupID groupID)
{
  pTaskHead pTemp, pEnd;

  assert(self);
  assert(self->iID == TASKERID);

  /* Do one cycle until we are at the caller, then return to him */
  pEnd = self->pCurrent;
  IncrTaskPointer(self);
  while (self->pCurrent != pEnd) {
    if (self->pCurrent->pSignal && self->pCurrent->groupID == groupID) {
      self->pCurrent->pSignal(self->pCurrent->pData, iSignal, pSigData);
    }
    IncrTaskPointer(self);
  }

  return 1;
}
/*-------------------------------------------------------------------------------*/
void TaskSetLogFunc(TaskLogFunc logFunc)
{
  TaskLogWrite = logFunc;
}
/*-------------------------------------------------------------------------------*/
TaskLogFunc TaskGetLogFunc(void)
{
  return TaskLogWrite;
}
/*-------------------------------------------------------------------------------*/
eTaskLogLevel TaskSetLogLevel(eTaskLogLevel logLevel)
{
  eTaskLogLevel old = TaskLogLevel;
  TaskLogLevel = logLevel;
  return old;
}
/*-------------------------------------------------------------------------------*/
eTaskLogLevel TaskGetLogLevel(void)
{
  return TaskLogLevel;
}
/*-------------------------------------------------------------------------------*/
int TaskQueueSet(pTaskMan self, TaskTaskID taskID, pTaskQueue pQueue)
{
  pTaskHead pt = TaskIteratorByID(self, taskID);
  if (pt) {
    pt->pQueue = pQueue;
    return 1;
  }
  return 0;
}

/*-------------------------------------------------------------------------------*/
int TaskQueueRem(pTaskMan self, TaskTaskID taskID)
{
  pTaskHead pt = TaskIteratorByID(self, taskID);
  if (pt) {
    pt->pQueue = NULL;
    return 1;
  }
  return 0;
}
/*-------------------------------------------------------------------------------*/
pTaskQueue TaskQueueGet(pTaskMan self, TaskTaskID taskID)
{
  pTaskHead pt = TaskIteratorByID(self, taskID);
  if (pt) {
    return pt->pQueue;
  }
  return NULL;
}

/*-------------------------------------------------------------------------------*/
pTaskQueue TaskQueueGetMine(pTaskMan self)
{
  pTaskHead pt = self->pCurrent;
  if (pt) {
    return pt->pQueue;
  }
  return NULL;
}

#define MAGIC_Q 12334
#define MAGIC_M 32441

typedef struct __TaskQueue {
  int magic;
  int count;
  pTaskMessage head;
  pTaskMessage tail;
} TaskQueue;

typedef struct __TaskMessage {
  int magic;
  int mType;
  pTaskMessage next;
  pTaskMessage prev;
  pTaskQueue owner;
  TaskTaskID lSenderID;
  void *pData;
} TaskMessage;

/*-------------------------------------------------------------------------------*/
static pTaskQueue taskQueueInit(pTaskQueue self)
{
  if (self) {
    if (self->magic == MAGIC_Q) {
      /* TODO */;
    }
    self->magic = MAGIC_Q;
    self->count = 0;
    self->head = NULL;
    self->tail = NULL;
  }
  return self;
}

/*-------------------------------------------------------------------------------*/
void TaskRunMeAfter(pTaskMan self, double delay)
{
  pTaskHead pThis = self->pCurrent;
  pThis->nextRunTime = DoubleTime() + delay;
  pThis->nextRunPeriod = 0.0;
}
/*-------------------------------------------------------------------------------*/
void TaskRunMeEvery(pTaskMan self, double delay)
{
  pTaskHead pThis = self->pCurrent;
  pThis->nextRunTime = DoubleTime() + delay;
  pThis->nextRunPeriod = delay;
}
/*-------------------------------------------------------------------------------*/
double TaskRunMyPeriod(pTaskMan self)
{
  pTaskHead pThis = self->pCurrent;
  return pThis->nextRunPeriod;
}
/*-------------------------------------------------------------------------------*/
pTaskQueue TaskQueueAlloc(void)
{
  pTaskQueue self = (pTaskQueue) calloc(1, sizeof(TaskQueue));
  if (self) {
    self = taskQueueInit(self);
  }
  return self;
}

/*-------------------------------------------------------------------------------*/
int TaskQueueFree(pTaskQueue self)
{
  if (self == NULL) {
    /* TODO */
    return -1;
  }
  if (self->magic != MAGIC_Q) {
    /* TODO */
    return -1;
  }
  if (self->count != 0 || self->head || self->tail) {
    /* TODO */
    return -1;
  }
  self->magic = 0;
  return 0;
}

/*-------------------------------------------------------------------------------*/
int TaskQueueCount(pTaskQueue self) {
  if (self == NULL) {
    /* TODO */
    return 0;
  }
  if (self->magic != MAGIC_Q) {
    /* TODO */
    return 0;
  }
  return self->count;
}

/*-------------------------------------------------------------------------------*/
int TaskQueueSend(pTaskQueue the_q, pTaskMessage the_m)
{
  if (the_q == NULL || the_m == NULL) {
    /* TODO */
    return 0;
  }
  if (the_q->magic != MAGIC_Q || the_m->magic != MAGIC_M) {
    /* TODO */
    return 0;
  }
  if (the_q->head) {
    the_m->prev = the_q->tail;
    the_q->tail->next = the_m;
  } else {
    the_q->head = the_m;
    the_m->prev = NULL;
  }
  the_q->tail = the_m;
  the_m->next = NULL;
  the_q->count++;
  the_m->owner = the_q;
  return the_q->count;
}

/*-------------------------------------------------------------------------------*/
int TaskQueueSendID(pTaskMan self, TaskTaskID taskID, pTaskMessage the_m)
{
  pTaskQueue the_q = TaskQueueGet(self, taskID);

  if (the_q == NULL)
    return 0;

  return TaskQueueSend(the_q, the_m);
}

/*-------------------------------------------------------------------------------*/
pTaskMessage TaskQueueRecv(pTaskQueue self)
{
  pTaskMessage m;
  if (self == NULL) {
    /* TODO */
    return NULL;
  }
  if (self->magic != MAGIC_Q) {
    /* TODO */
    return NULL;
  }
  if (self->count <= 0 || !self->head || !self->tail) {
    /* TODO */
    return NULL;
  }
  self->count--;
  m = self->head;
  self->head = self->head->next;
  if (self->head == NULL) {
    self->tail = NULL;
    if (self->count) {
      /* TODO */
      self->count = 0;
    }
  }
  m->next = m->prev = NULL;
  return m;
}

/*-------------------------------------------------------------------------------*/
pTaskMessage TaskQueueRecvMine(pTaskMan self)
{
  return TaskQueueRecv(self->pCurrent->pQueue);
}
/*-------------------------------------------------------------------------------*/
static pTaskMessage taskMessageInit(pTaskMessage self, int mType)
{
  if (self) {
    if (self->magic == MAGIC_M) {
      /* TODO */;
    }
  self->magic = MAGIC_M;
  self->next = NULL;
  self->prev = NULL;
  self->owner = NULL;
  self->mType = mType;
  self->pData = NULL;
  }
  return self;
}

/*-------------------------------------------------------------------------------*/
pTaskMessage TaskMessageAlloc(size_t mSize, int mType)
{
  pTaskMessage self = (pTaskMessage) calloc(1, sizeof(TaskMessage) + mSize);
  if (self) {
    self = taskMessageInit(self, mType);
    TaskMessageSetData(self, &self[1]); /* pointer to data beyond this structure */
  }
  return self;
}

/*-------------------------------------------------------------------------------*/
int TaskMessageFree(pTaskMessage self) {
  if (self == NULL) {
    /* TODO */
    return -1;
  }
  if (self->magic != MAGIC_M) {
    /* TODO */
  }
  if (self->pData) {
    /* TODO */
  }
  self->magic = 0;
  free(self);
  return 0;
}

/*-------------------------------------------------------------------------------*/
int TaskMessageGetType(pTaskMessage self)
{
  if (self)
    return self->mType;
  return 0;
}

/*-------------------------------------------------------------------------------*/
void TaskMessageSetType(pTaskMessage self, int mType)
{
  if (self)
    self->mType = mType;
}

/*-------------------------------------------------------------------------------*/
void * TaskMessageGetData(pTaskMessage self)
{
  if (self)
    return self->pData;
  return NULL;
}

/*-------------------------------------------------------------------------------*/
void TaskMessageSetData(pTaskMessage self, void *pData)
{
  if (self)
    self->pData = pData;
}