/*
 * Hack by George Peter Staplin
 */

#include <stdio.h>
#include <stdlib.h>
#include <setjmp.h>
#include <signal.h>
#include <unistd.h>
#include <string.h>
#include <ucontext.h>
#include <machine/mcontext.h>

struct task {
 int has_run;
 ucontext_t context;
 void (*callback) (struct task *t);
 void *ptr;
};

struct tasklist {
 struct tasklist *prev;
 struct task *task;
 struct tasklist *next;
};

static struct tasklist *tasks = NULL;
static struct tasklist *taskptr = NULL;
static ucontext_t schedulercontext;
static int counter = 0;

#define STACKSIZE 1024

struct task *createtask (void (*callback) (struct task *t)) {
 struct task *t;
 struct tasklist *tlist;
 char *stack;

 t = malloc (sizeof *t);

 if (NULL == t) {
  perror ("createtask");
  exit (EXIT_FAILURE);
 }

 t->has_run = 0;

 if (getcontext (&t->context)) {
  perror ("getcontext");
  exit (EXIT_FAILURE);
 }

 stack = malloc (STACKSIZE);
 if (NULL == stack) {
  perror ("malloc stack");
  exit (EXIT_FAILURE);
 }

 t->context.uc_stack.ss_sp = stack;
 t->context.uc_stack.ss_size = STACKSIZE;
 t->context.uc_link = NULL;
 makecontext (&t->context, callback, 1, t);
 
 t->callback = callback;
 t->ptr = NULL;

 /*
  * Now link the task into the tasklist.
  */
 tlist = malloc (sizeof *tlist);
 if (NULL == tlist) {
  perror ("allocating tasklist");
  exit (EXIT_FAILURE);
 }

 tlist->task = t;

 if (NULL == tasks) {
  tlist->prev = tlist;
  tlist->next = tlist;
  tasks = tlist;
 } else {
  tlist->next = tasks;
  tlist->prev = tasks->prev;

  if (tasks->next == tasks) {
   tasks->next = tlist;
  }
  tasks = tlist;
 }

 return t;
}

void yieldtask (struct task *t) {
 int localcount = counter;

 if (getcontext (&t->context)) {
  perror ("yieldtask");
  exit (EXIT_FAILURE);
 }

 if (localcount == counter) {
  setcontext (&schedulercontext);
 }
}


void a (struct task *t) {
 int a = 0;
 while (1) {
  printf ("a %d\n", a);
  ++a;

  if (0 == a % 2) {
   yieldtask (t);
  }
 }
}

void b (struct task *t) {
 int b = 0;
 while (1) {
  printf ("b %d\n", b);
  ++b;

  if (0 == b % 9) {
   yieldtask (t);
  }
 }
}

void roundrobin (void) {
 getcontext (&schedulercontext);

 ++counter;

 taskptr = taskptr->next;
 setcontext (&taskptr->task->context);
}

int main () {
 createtask (a);
 createtask (b);

 taskptr = tasks;
 roundrobin ();

 return EXIT_SUCCESS;
}