#ifndef CORE_MATRIX_STACK_H
#define CORE_MATRIX_STACK_H

#include "data/ArrayList.h"
#include "math/Matrix.h"

namespace Core {
    template<int N>
    class MatrixStack final {
        ArrayList<Matrix, N> stack;

    public:
        MatrixStack() {
            (void)stack.add(Matrix());
        }

        check_return Error pop() {
            if(stack.getLength() <= 1) {
                return Error::INVALID_STATE;
            }
            return stack.removeLast();
        }

        check_return Error push() {
            return stack.add(peek());
        }

        Matrix& peek() {
            return stack[stack.getLength() - 1];
        }

        const Matrix& peek() const {
            return stack[stack.getLength() - 1];
        }

        void clear() {
            stack.clear();
            (void)stack.add(Matrix());
        }

        template<int L>
        check_return Error toString(ArrayString<L>& s) const {
            return s.append(stack);
        }
    };
}

#endif