#ifdef __cplusplus
# include <deque>
/* using std::deque */
# include <new>
/* using placement new operator */
#endif

#include <stdlib.h>
/* using malloc, free, rand */
#include <time.h>
/* using clock_gettime */
#include <stdio.h>
/* using printf, puts */
#include <errno.h>
/* using errno */
#include <string.h>
/* using memcpy */


/** Abstract object for different queue implementations */
struct QueueObject;

/** Class metadata for queue implementations */
struct QueueInterface
{
  /** Human-readable class name */
  const char* name;
  /** Allocation size */
  size_t size;
  /** Constructor. Returns 0 on success */
  int (*init)(struct QueueObject*);
  /** Destructor. Does not free the object pointer itself */
  void (*free)(struct QueueObject*);
  /** Adds the given pointer to the end of the queue. Returns 0 on success */
  int (*enqueue)(struct QueueObject*, void*);
  /** Returns the first object in the queue or NULL if empty */
  void* (*dequeue)(struct QueueObject*);
};


#ifdef __cplusplus

typedef std::deque<void*> _cppqueue_deque_type;

/** C-Wrapper around C++'s STL deque */
struct CppQueue
{
  _cppqueue_deque_type deque;
};

static int cppqueue_init(struct QueueObject* vself)
{
  struct CppQueue* self = (struct CppQueue*) vself;
  new (&self->deque) _cppqueue_deque_type;
  return 0;
}

static void cppqueue_free(struct QueueObject* vself)
{
  struct CppQueue* self = (struct CppQueue*) vself;
  self->deque.~_cppqueue_deque_type();
}

static int cppqueue_enqueue(struct QueueObject* vself, void* element)
{
  struct CppQueue* self = (struct CppQueue*) vself;
  try {
    self->deque.push_back(element);
  } catch(...) {
    return 1;
  }
  return 0;
}

static void* cppqueue_dequeue(struct QueueObject* vself)
{
  struct CppQueue* self = (struct CppQueue*) vself;
  void* element = NULL;
  if(self->deque.empty())
    return element;
  element = self->deque.front();
  self->deque.pop_front();
  return element;
}

static const struct QueueInterface CPP_QUEUE_INTERFACE = {
  "CppQueue",
  sizeof(struct CppQueue),
  cppqueue_init,
  cppqueue_free,
  cppqueue_enqueue,
  cppqueue_dequeue
};

#endif /* __cplusplus */

/** Queue based on an exponentially growing array treated as a ring buffer */
struct Queue
{
  /** Points to beginning of the array or NULL if not yet allocated */
  void** ring;
  /**
   * Points to the first element in the queue
   *
   * If it points to writepos, the queue is empty
   */
  void** readpos;
  /** Points behind the last entry in the queue
   *
   * If it points to readpos - 1, the queue is full. Note that this means that
   * one element in the array can never be used
   */
  void** writepos;
  /**
   * Points behind the end of the ring array
   *
   * If readpos or writepos reach this position, they have to wrap around to
   * the beginning
   */
  void** ringend;
};

static int queue_init(struct QueueObject* vself)
{
  struct Queue* self = (struct Queue*) vself;
  /** Allocation happens on first access */
  self->ring = self->readpos = self->writepos = self->ringend = NULL;
  return 0;
}

static void queue_free(struct QueueObject* vself)
{
  struct Queue* self = (struct Queue*) vself;
  free(self->ring);
}

/** Returns the number of elements in the queue */
static size_t _queue_count(const struct Queue* self)
{
  if(self->writepos >= self->readpos)
    return self->writepos - self->readpos;
  else
    return (self->ringend - self->readpos) + (self->writepos - self->ring);
}

/** Grows the size of the ring buffer. Returns 0 on success */
static int _queue_extend(struct Queue* self)
{
  size_t newsize;
  void** newring;
  size_t count;
  newsize = (self->ringend - self->ring + 1) * 2;
  if(! (newring = (void**) malloc(newsize * sizeof(void*))))
    return 1;
  count = _queue_count(self);
  if(self->writepos >= self->readpos)
    self->readpos = (void**) memcpy(newring, self->readpos,
				    count * sizeof(void*));
  else {
    size_t before_end = self->ringend - self->readpos;
    self->readpos = (void**) memcpy(newring, self->readpos,
				    before_end * sizeof(void*));
    memcpy(newring + before_end, self->ring,
	   (count - before_end) * sizeof(void*));
  }
  free(self->ring);
  self->ring = newring;
  self->writepos = newring + count;
  self->ringend = newring + newsize;
  return 0;
}

static int queue_enqueue(struct QueueObject* vself, void* element)
{
  struct Queue* self = (struct Queue*) vself;
  if(self->writepos == self->readpos - 1 || ! self->writepos) {
    int err;
    if((err = _queue_extend(self)) != 0)
      return err;
  }
  *self->writepos = element;
  if(++self->writepos == self->ringend)
    self->writepos = self->ring;
  return 0;
}

void* queue_peek(const struct Queue* self)
{
  return self->readpos == self->writepos ? NULL : *self->readpos;
}

static void* queue_dequeue(struct QueueObject* vself)
{
  struct Queue* self = (struct Queue*) vself;
  void* element = NULL;
  if(self->readpos == self->writepos)
    return element;
  element = *self->readpos;
  if(++self->readpos == self->ringend)
    self->readpos = self->ring;
  return element;
}

static const struct QueueInterface AQUEUE_INTERFACE = {
  "Queue",
  sizeof(struct Queue),
  queue_init,
  queue_free,
  queue_enqueue,
  queue_dequeue
};


/** Single linked list entry class */
struct LinkedElement
{
  /** next element in list or NULL */
  struct LinkedElement* next;
  /** Whatever the node points to */
  void* element;
};

/** Single linked header */
struct SLList
{
  /** First and last nodes in the list or NULL if list is empty */
  struct LinkedElement* front, *back;
};

/** Initializes an empty list */
static int sllist_init(struct SLList* self)
{
  self->front = self->back = NULL;
  return 0;
}

/** Deallocates all nodes in the list */
static void sllist_free(struct SLList* self)
{
  struct LinkedElement* cur, *next;
  for(cur = self->front; cur != NULL; cur = next) {
    next = cur->next;
    free(cur);
  }
}

/** Adds a single node to the end of the list */
static void sllist_push_back(struct SLList* self, struct LinkedElement* node)
{
  node->next = NULL;
  if(self->back)
    self->back->next = node;
  else
    self->front = node;
  self->back = node;
}

/** Adds a single node to the front of the list */
static void sllist_push_front(struct SLList* self, struct LinkedElement* node)
{
  if(! (node->next = self->front))
    self->back = node;
  self->front = node;
}

/**
 * Allocates a new node and adds it to the back of the list
 *
 * Returns NULL on error
 */
static struct LinkedElement* sllist_alloc_back(struct SLList* self)
{
  struct LinkedElement* node;
  if(! (node = (struct LinkedElement*) malloc(sizeof(struct LinkedElement))))
    return node;
  sllist_push_back(self, node);
  return node;
}

/**
 * Removes the first element from the list and returns it
 *
 * Returns NULL if empty
 */
static struct LinkedElement* sllist_pop_front(struct SLList* self)
{
  struct LinkedElement* node;
  if(! (node = self->front))
    return node;
  if(! (self->front = node->next))
    self->back = NULL;
  return node;
}


/**
 * Queue implementation based on a single linked list
 *
 * Each enqueue results in an malloc
 */
struct SLQueue
{
  struct SLList list;
};

static int slqueue_init(struct QueueObject* vself)
{
  struct SLQueue* self = (struct SLQueue*) vself;
  return sllist_init(&self->list);
}

static void slqueue_free(struct QueueObject* vself)
{
  struct SLQueue* self = (struct SLQueue*) vself;
  sllist_free(&self->list);
}

static int slqueue_enqueue(struct QueueObject* vself, void* element)
{
  struct SLQueue* self = (struct SLQueue*) vself;
  struct LinkedElement* node;
  if(! (node = sllist_alloc_back(&self->list)))
    return 1;
  node->element = element;
  return 0;
}

void* slqueue_peek(const struct SLQueue* self)
{
  return self->list.front ? self->list.front->element : NULL;
}

static void* slqueue_dequeue(struct QueueObject* vself)
{
  struct SLQueue* self = (struct SLQueue*) vself;
  struct LinkedElement* node;
  void* element = NULL;
  if(! (node = sllist_pop_front(&self->list)))
    return element;
  element = node->element;
  free(node);
  return element;
}

static const struct QueueInterface SLQUEUE_INTERFACE = {
  "SLQueue",
  sizeof(struct SLQueue),
  slqueue_init,
  slqueue_free,
  slqueue_enqueue,
  slqueue_dequeue
};


/**
 * Queue implementation based on a single linked list
 *
 * Allocations are cached in a second list to be reused
 */
struct CSLQueue
{
  struct SLList contained, freelist;
};

static int cslqueue_init(struct QueueObject* vself)
{
  struct CSLQueue* self = (struct CSLQueue*) vself;
  struct SLList* lists[] = {&self->contained, &self->freelist};
  int err, i, _errno;
  for(i = err = 0; i < 2 && ! err; ++i)
    err = sllist_init(lists[i]);
  if(! err)
    return err;
  _errno = errno;
  for(--i; i >= 0; --i)
    sllist_free(lists[i]);
  errno = _errno;
  return err;
}

static void cslqueue_free(struct QueueObject* vself)
{
  struct CSLQueue* self = (struct CSLQueue*) vself;
  struct SLList* lists[] = {&self->contained, &self->freelist};
  int i;
  for(i = 0; i < 2; ++i)
    sllist_free(lists[i]);
}

static int cslqueue_enqueue(struct QueueObject* vself, void* element)
{
  struct CSLQueue* self = (struct CSLQueue*) vself;
  struct LinkedElement* node;
  if(! (node = sllist_pop_front(&self->freelist))) {
    if(! (node = sllist_alloc_back(&self->contained)))
      return 1;
  }
  else
    sllist_push_back(&self->contained, node);
  node->element = element;
  return 0;
}

void* cslqueue_peek(const struct CSLQueue* self)
{
  return self->contained.front ? self->contained.front->element : NULL;
}

static void* cslqueue_dequeue(struct QueueObject* vself)
{
  struct CSLQueue* self = (struct CSLQueue*) vself;
  struct LinkedElement* node;
  void* element = NULL;
  if(! (node = sllist_pop_front(&self->contained)))
    return element;
  element = node->element;
  sllist_push_front(&self->freelist, node);
  return element;
}

static const struct QueueInterface CSLQUEUE_INTERFACE = {
  "CSLQueue",
  sizeof(struct CSLQueue),
  cslqueue_init,
  cslqueue_free,
  cslqueue_enqueue,
  cslqueue_dequeue
};


/**
 * Dummy queue that does nothing. Used to get base values for the benchmark
 */
struct NoQueue
{
  char dummy;
};

static int noqueue_init(struct QueueObject* vself)
{ return 0; }

static void noqueue_free(struct QueueObject* vself)
{}

static int noqueue_enqueue(struct QueueObject* vself, void* unused)
{ return 0; }

static void* noqueue_dequeue(struct QueueObject* vself)
{ return vself; }

static const struct QueueInterface NOQUEUE_INTERFACE = {
  "NoQueue",
  sizeof(struct NoQueue),
  noqueue_init,
  noqueue_free,
  noqueue_enqueue,
  noqueue_dequeue
};


/**
 * Returns 0 if the current operation should be stopped
 *
 * \param flippow scales the probability of a true result by its power of two
 */
static int continue_operation(unsigned flippow)
{
  return rand() & ((1 << flippow) - 1);
}

int main()
{
  /* queue implementations to be tested */
  static const struct QueueInterface* interfaces[] = {
#  ifdef __cplusplus
    &CPP_QUEUE_INTERFACE,
#  endif
    &AQUEUE_INTERFACE,
    &SLQUEUE_INTERFACE,
    &CSLQUEUE_INTERFACE,
    &NOQUEUE_INTERFACE
  };
  /* Churn defines how often filling and clearing the queue alternate.
   * High churn values mean there is a high fluctuation in size of the queue
   */
  unsigned churn;
  puts("Churn\tImplementation\tTime per op[s]");
  for(churn = 1; churn <= 10; ++churn) {
    size_t i;
    for(i = 0; i < sizeof(interfaces) / sizeof(*interfaces); ++i) {
      const struct QueueInterface* interface;
      struct QueueObject* obj;
      int err = 0;
      struct timespec starttime, endtime;
      double spenttime, time_per_op;
      int j;
      /* ops counts the enqueue, dequeue operations */
      unsigned ops = 0;
      interface = interfaces[i];
      if(! (obj = (struct QueueObject*) malloc(interface->size))) {
	err = 1;
	goto rtrn;
      }
      if((err = interface->init(obj)) != 0)
	goto obj_free;
      if((err = clock_gettime(CLOCK_MONOTONIC, &starttime)) != 0)
	goto obj_dtor;
      /** Do a few thousand enque, dequeue iterations */
      for(j = 0; j < ((1<<26) >> churn); ++j) {
	do {
	  ++ops;
	  if(interface->enqueue(obj, obj))
	    break;
	} while(continue_operation(churn));
	do {
	  ++ops;
	  if(! interface->dequeue(obj))
	    break;
	} while(continue_operation(churn));
      }
      if((err = clock_gettime(CLOCK_MONOTONIC, &endtime)) != 0)
	goto obj_dtor;
      spenttime = difftime(endtime.tv_sec, starttime.tv_sec)
	+ (endtime.tv_nsec - starttime.tv_nsec) * 1e-9;
      time_per_op = spenttime / ops;
      if(printf("%u\t%-14s\t%#g\n", 1 << churn, interface->name, time_per_op)
	 < 0) {
	err = 1;
	goto obj_dtor;
      }
    obj_dtor:
      interface->free(obj);
    obj_free:
      free(obj);
    rtrn:
      if(err)
	return err;
    }
  }
  return 0;
}