#ifndef TPQ_H
#define TPQ_H

#include <Arduino.h>

template<class TP>
class TPQ {
  private:
    int _front, _back, _count;
    TP *_data;
    int _maxitems;
  public:
    TPQ(int maxitems = 256) { 
      _front = 0;
      _back = 0;
      _count = 0;
      _maxitems = maxitems;
      _data = new TP[maxitems + 1];   
    }
    ~TPQ() {
      delete[] _data;  
    }
    void push(const TP &item);
    TP peek_item(const TP &item);
};



template<class TP>
void TPQ<TP>::push(const TP &item)
{
  if(_count < _maxitems) { // Drops out when full
    _data[_back++]=item;
    ++_count;
    // Check wrap around
    if (_back > _maxitems)
      _back -= (_maxitems + 1);
  }
}




template<class TP>
TP TPQ<TP>::peek_item(const TP &item) {
  if(_count <= 0) return TP(); // Returns empty
  else { 
    TP result = item;// T result = _data[_front];
    //_front++;
    //--_count;
     if (_front > _maxitems) 
      _front -= (_maxitems + 1);
  return result;
  }
}

#endif