#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "msgqueue.h"

//#define QUEUE_DEBUG

//These three don't lock, and are meant to be called by a function that has already locked the queue.
static int local_msg_is_empty(struct MsgQueue * fq);
static int local_msg_is_full(struct MsgQueue * fq);
static int local_msg_length(struct MsgQueue * fq);

void msg_init(struct MsgQueue * fq, int s)
{
  fq->front = 0;
  fq->back = -1;
  fq->size = s;
  //fq->peek_p = 0;

  fq->queue = (void**)malloc(sizeof(void*) * s);
  if(!fq->queue)
    {
      printf("Queue malloc failed\n");
      exit(-1);
    }

  fq->cond = (pthread_cond_t*)malloc(sizeof(pthread_cond_t));
  fq->cond_lock = (pthread_mutex_t*)malloc(sizeof(pthread_mutex_t));

  fq->lock = (pthread_mutex_t*)malloc(sizeof(pthread_mutex_t));

  pthread_cond_init(fq->cond, 0);
  pthread_mutex_init(fq->cond_lock, 0);

  pthread_mutex_init(fq->lock, 0);
}

void * msg_pop(struct MsgQueue * fq)
{
  void * ret;

  if (msg_is_empty(fq)) 
    {
      pthread_mutex_lock(fq->cond_lock);
      pthread_cond_wait(fq->cond, fq->cond_lock);
      pthread_mutex_unlock(fq->cond_lock);
    }

  pthread_mutex_lock(fq->lock);

  ret = fq->queue[fq->front];

  fq->front ++;

  if (fq->front == fq->size) fq->front = 0;

  pthread_mutex_unlock(fq->lock);

  return ret;
}

int msg_push(struct MsgQueue * fq, void * c)
{

  if (msg_is_full(fq)) return QUEUE_FULL;

  pthread_mutex_lock(fq->lock);

  if( fq->back == (fq->size-1)) fq->back = -1;

  fq->back ++;

  fq->queue[fq->back] = c;

  pthread_mutex_unlock(fq->lock);

  pthread_mutex_lock(fq->cond_lock);
  pthread_cond_broadcast(fq->cond);
  pthread_mutex_unlock(fq->cond_lock);

  return QUEUE_OK;
}


int msg_is_empty(struct MsgQueue * fq)
{
  int ret;

  pthread_mutex_lock(fq->lock);
  ret =  (((fq->back+1) == fq->front) || ((fq->front+fq->size-1) == fq->back ));
  pthread_mutex_unlock(fq->lock); 
  return ret;
}

int msg_is_full(struct MsgQueue * fq)
{
  int ret; 

  pthread_mutex_lock(fq->lock);
  ret =  ( ((fq->back+2) == fq->front) || ((fq->front + fq->size-2) == fq->back) );
  pthread_mutex_unlock(fq->lock);
  return ret;
}

int msg_length(struct MsgQueue * fq)
{
  int retval;
#ifdef QUEUE_DEBUG
  printf("f: %d b: %d\n", fq->front, fq->back);
#endif

  pthread_mutex_lock(fq->lock);

  if(local_msg_is_empty(fq)==1) return 0;

  if (fq->back == -1) 
    { retval = fq->size; }
  else 
    { retval = fq->back+1; }

  if (fq->back >= fq->front) 
    retval = fq->back - fq->front + 1 ;
  else 
    retval = ( fq->size - fq->front) + (retval);

  pthread_mutex_unlock(fq->lock);

  return(retval);
}

void msg_print(struct MsgQueue * fq)
{
  printf("size: %d ", fq->size);
  printf("front: %d ", fq->front);
  printf("back: %d ", fq->back);
  //  printf("peek_p: %d\n", fq->peek_p);
}


static int local_msg_is_empty(struct MsgQueue * fq)
{
  int ret;
  ret =  (((fq->back+1) == fq->front) || ((fq->front+fq->size-1) == fq->back ));
  return ret;
}

static int local_msg_is_full(struct MsgQueue * fq)
{
  int ret; 
  ret =  ( ((fq->back+2) == fq->front) || ((fq->front + fq->size-2) == fq->back) );
  return ret;
}

static int local_msg_length(struct MsgQueue * fq)
{
  int retval;
#ifdef QUEUE_DEBUG
  printf("f: %d b: %d\n", fq->front, fq->back);
#endif

  if(msg_is_empty(fq)==1) return 0;

  if (fq->back == -1) 
    { retval = fq->size; }
  else 
    { retval = fq->back+1; }

  if (fq->back >= fq->front) 
    retval = fq->back - fq->front + 1 ;
  else 
    retval = ( fq->size - fq->front) + (retval);

  return(retval);
}

/*
struct frame * msg_view_pos(struct MsgQueue * fq, int pos)
{
  int index;
  if (msg_length(fq) <= pos) return 0;

  if ( (fq->front + pos) < fq->size) 
    {
      index = (fq->front + pos);
    }
  else
    {
      index = pos - (fq->size-fq->front);
    }

  return &(fq->queue[index]);
}

struct frame * msg_peek(struct MsgQueue * fq)
{
  struct frame * ret ;
  ret = msg_view_pos(fq, fq->peek_p);
  fq->peek_p += 1;
  if (ret == 0) fq->peek_p -= 1;
  return ret;
}

void msg_reset_peek(struct MsgQueue * fq)
{
  fq->peek_p = 0;
}
*/