/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */
/** 
 * A bounded queue has the same interface as a normal queue
 * (i.e. "enqueue" and "dequeue" operations) but is created with an
 * upper bound on its capacity.  If the bounded queue is full,
 * subsequent "enqueue" operations will first call "dequeue" to get
 * rid of the first element in the queue.
 */

#ifndef CPMLIB_BOUNDEDQUEUE_H
#define CPMLIB_BOUNDEDQUEUE_H

#include <stdexcept>

template<class T> class BoundedQueue {
public:
  BoundedQueue(int capacity) {
    contents = new T[capacity];
    front = 0;
    msize = 0;
    mcapacity = capacity;
  }

  BoundedQueue(const BoundedQueue &other) {
    contents = new T[other.mcapacity];
    for(int i=0; i<other.mcapacity; i++)
      contents[i] = other.contents[i];
    
    front = other.front;
    msize = other.msize;
    mcapacity = other.mcapacity;
  }

  BoundedQueue(const BoundedQueue *other) {
    contents = new T[other->mcapacity];
    for(int i=0; i<other->mcapacity; i++)
      contents[i] = other->contents[i];
    
    front = other->front;
    msize = other->msize;
    mcapacity = other->mcapacity;
  }

  virtual ~BoundedQueue() {
    delete[] contents;
    contents = NULL;
  }

  virtual T get(int index) {
    if (index < 0 || index >= msize) {
      throw std::out_of_range("Out of bounds in BoundedQueue::get(int)");
    }
    return contents[(front + index) % mcapacity];
  }

  int capacity() {
    return mcapacity;
  }

  virtual int size() {
    return msize;
  }
  
  virtual void enqueue(T elt) {
    if (msize == mcapacity) {
      dequeue();
    }
    contents[(front + msize) % mcapacity] = elt;
    msize++;
  }
  
  T dequeue() {
    T retval = get(0);
    front++;
    front = front % mcapacity;
    msize--;
    return retval;
  }
  
private:
  T *contents;
  int front;
  int msize;
  int mcapacity;
};

#endif