#include <string.h>
#ifndef USE_PTHREAD

#include "thread.h"
#include "debug.h"
#include "pthread.h"
#include <sys/queue.h>
#include <bits/pthreadtypes.h>
#include <stdlib.h>
#include <ucontext.h>
#include <valgrind/valgrind.h>

#define FINISHED  0x1
#define ALLOCATED 0x2

#ifndef STACK_SIZE
#define STACK_SIZE 4096*4
#endif

struct context_entry {
  TAILQ_ENTRY(context_entry) link;
  ucontext_t context;
  thread_t id;
  thread_t parent;
  void *retvalue;
  int valgrind_id;
  char status;
};

static TAILQ_HEAD(context_head, context_entry) head = TAILQ_HEAD_INITIALIZER(head);
static struct context_entry *running = 0;
static unsigned long long counter = 0;

int thread_yield(void) {
  if (counter <= 1) {
    return 0;
  }
  struct context_entry *first;
  int count = 0;
  do  {
    if (count++ == counter) {
      return 0;
    }
    first = TAILQ_FIRST(&head);
    if (!first) {
      return -1;
    }
    TAILQ_REMOVE(&head, first, link);
    TAILQ_INSERT_TAIL(&head, first, link);
    if (first->id == running->id) {
      return 0;
    }
  } while (first->status & FINISHED);
  struct context_entry *old_runner = running;
  running = first;
  swapcontext(&old_runner->context, &running->context);
  return 0;
}

thread_t thread_self(void) {
  if (running == NULL) {
    return 0;
  }
  return running->id;
}

int thread_create(thread_t *newthread, void *(*func)(void *), void *funcarg) {
  TRACE("Create a new thread that execute function %p", func);
  if (counter == 0) {
    struct context_entry *main = malloc(sizeof(*main));
    memset(main, 0, sizeof(*main));
    getcontext(&main->context);
    main->id = 0;
    main->parent = 0;
    main->status = 0;
    main->retvalue = 0;
    TAILQ_INSERT_HEAD(&head, main, link);
    running = main;
    counter++;
  }
  struct context_entry *new_entry = malloc(sizeof(*new_entry));
  memset(new_entry, 0, sizeof(*new_entry));
  getcontext(&new_entry->context);
  new_entry->context.uc_stack.ss_sp = malloc(STACK_SIZE);
  new_entry->context.uc_stack.ss_size = STACK_SIZE;
  new_entry->valgrind_id = VALGRIND_STACK_REGISTER(
    new_entry->context.uc_stack.ss_sp,
    new_entry->context.uc_stack.ss_sp + new_entry->context.uc_stack.ss_size
    );
  new_entry->id = new_entry;
  new_entry->parent = running->id;
  new_entry->status = ALLOCATED;
  new_entry->retvalue = 0;
  *newthread = new_entry;
  makecontext(&new_entry->context, (void (*)()) func, 1, funcarg);
  counter++;
  TAILQ_INSERT_HEAD(&head, new_entry, link);
  return 0;
}

void print_entry(struct context_entry *entry) {
  TRACE("CONTEXT (%p, %p, %d, %p);\n", entry, entry->id, (entry->status & FINISHED) > 0, entry->parent);
}

int thread_join(thread_t thread, void **retval) {
  TRACE("Join thread %p", thread);
  struct context_entry *entry;
  TAILQ_FOREACH(entry, &head, link) {
    if (entry->id != thread) {
      continue;
    }
    TRACE("FIND");
    print_entry(entry);
    while (!entry->status & FINISHED) {
      TRACE("NOT FINISHED");
      thread_yield();
    }
    TRACE("AFTER");
    if (retval)
      *retval = entry->retvalue;
    TAILQ_REMOVE(&head, entry, link);
    if (entry->status & ALLOCATED) {
      TRACE("FREE\n");
      VALGRIND_STACK_DEREGISTER(entry->valgrind_id);
      free(entry->context.uc_stack.ss_sp);
    } else {
      TRACE("NOT ALLOCATED\n");
    }
    free(entry);
    if (--counter == 1) {
      TAILQ_REMOVE(&head, running, link);
      if (running->status & ALLOCATED) {
        VALGRIND_STACK_DEREGISTER(running->valgrind_id);
        free(running->context.uc_stack.ss_sp);
      }
      free(running);
      running = 0;
      counter = 0;
    }
    return 0;
  }
  return -1;
}

void thread_exit(void *retval) {
  TRACE("Exit thread %p", running);
  running->status |= FINISHED;
  running->retvalue = retval;
  thread_yield();
  exit(0);
}

int thread_mutex_init(thread_mutex_t *mutex) {
  return pthread_mutex_init((pthread_mutex_t *) mutex, NULL);
}
int thread_mutex_destroy(thread_mutex_t *mutex) {
  return pthread_mutex_destroy((pthread_mutex_t *) mutex);
}
int thread_mutex_lock(thread_mutex_t *mutex) {
  return pthread_mutex_lock((pthread_mutex_t * )mutex);
}
int thread_mutex_unlock(thread_mutex_t *mutex) {
  return pthread_mutex_unlock((pthread_mutex_t *)mutex);
}

#endif