#ifndef Stack_H
#define Stack_H
#include <Arduino.h>

template<typename T>
class Stack {
  public:
    Stack();
    ~Stack();
    void push(const T);
    T pop();
    T peek();
    int stackCount();
  private:
    bool isEmpty();
    bool isFull();
    void resize(int);
    static const int defSize = 4;
    T* stackPointer;
    int sSize, top;
};
template<typename T>
Stack<T>::Stack(){
  top = 0;
  stackPointer = (T*)malloc(sizeof(T) * defSize);
  if(stackPointer == NULL) {
    if(Serial) {
      Serial.println("Could not create stack");
    }
  }
  sSize = defSize;
}
template<typename T>
Stack<T>::~Stack() {
  free(stackPointer);
}
template<typename T>
void Stack<T>::push(const T t){
  if(isFull()) {
    resize(sSize * 2);
  }
  stackPointer[top++] = t;
}
template<typename T>
T Stack<T>::pop() {
  if(isEmpty()) {
    if(Serial) {
      Serial.println("Stack pop failed as empty");
    }
  }
  T t = stackPointer[--top];
  // think about resizing
  if(top <= sSize / 4 && sSize > defSize) {
    resize(sSize / 2);
  }
  return t;
}
template<typename T>
T Stack<T>::peek() {
  if(isEmpty()) {
    if(Serial) {
      Serial.println("No items in stack");
    }
  } else {
    T t = stackPointer[top - 1];
    return t;
  }
}
template<typename T>
int Stack<T>::stackCount() {
  return top;
}

// now the private methods
template<typename T>
bool Stack<T>::isEmpty() {
  return (top == 0);
}
template<typename T>
bool Stack<T>::isFull() {
  return (top == sSize);
}
template<typename T>
void Stack<T>::resize(int newSize) {
  stackPointer = (T *)realloc(stackPointer, sizeof(T) * newSize);
  if(stackPointer == NULL) {
    if(Serial) {
      Serial.println("Stack resize failed");
    }
  }
  sSize = newSize;
}
#endif