#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.

template <typename T>
MsgQueue<T>::MsgQueue(int s)
{
  front = 0;
  back = -1;
  size = s;

  queue = new T* [s];
  if(!queue)
    {
      printf("Queue malloc failed\n");
      exit(-1);
    }

  cond = (pthread_cond_t*)malloc(sizeof(pthread_cond_t));
  cond_lock = (pthread_mutex_t*)malloc(sizeof(pthread_mutex_t));

  lock = (pthread_mutex_t*)malloc(sizeof(pthread_mutex_t));

  pthread_cond_init(cond, 0);
  pthread_mutex_init(cond_lock, 0);

  pthread_mutex_init(lock, 0);
}


template <typename T>
MsgQueue<T>::~MsgQueue()
{
  delete [] queue;
}

template <typename T>
T * MsgQueue<T>::pop()
{
  T * ret;

  if (is_empty()) 
    {
      pthread_mutex_lock(cond_lock);
      pthread_cond_wait(cond, cond_lock);
      pthread_mutex_unlock(cond_lock);
    }

  pthread_mutex_lock(lock);

  ret = queue[front];

  front ++;

  if (front == size) front = 0;

  pthread_mutex_unlock(lock);

  return ret;
}

template <typename T>
int MsgQueue<T>::push(T * c)
{

  if (is_full()) return QUEUE_FULL;

  pthread_mutex_lock(lock);

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

  back ++;

  queue[back] = c;

  pthread_mutex_unlock(lock);

  pthread_mutex_lock(cond_lock);
  pthread_cond_broadcast(cond);
  pthread_mutex_unlock(cond_lock);

  return QUEUE_OK;
}

template <typename T>
int MsgQueue<T>::is_empty()
{
  int ret;

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

template <typename T>
int MsgQueue<T>::is_full()
{
  int ret; 

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

template <typename T>
int MsgQueue<T>::length()
{
  int retval;
#ifdef QUEUE_DEBUG
  printf("f: %d b: %d\n", front, back);
#endif

  pthread_mutex_lock(lock);

  if(local_is_empty()==1) return 0;

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

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

  pthread_mutex_unlock(lock);

  return(retval);
}

template <typename T>
void MsgQueue<T>::print()
{
  printf("size: %d ", size);
  printf("front: %d ",front);
  printf("back: %d ", back);
  //  printf("peek_p: %d\n", peek_p);
}

template <typename T>
int MsgQueue<T>::local_is_empty()
{
  int ret;
  ret =  (((back+1) == front) || ((front+size-1) == back ));
  return ret;
}

template <typename T>
int MsgQueue<T>::local_is_full()
{
  int ret; 
  ret =  ( ((back+2) == front) || ((front + size-2) == back) );
  return ret;
}

template <typename T>
int MsgQueue<T>::local_length()
{
  int retval;
#ifdef QUEUE_DEBUG
  printf("f: %d b: %d\n", front, back);
#endif

  if(is_empty()==1) return 0;

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

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

  return(retval);
}