core/include/core/math/MatrixStack.hpp

#ifndef CORE_MATRIX_STACK_HPP
#define CORE_MATRIX_STACK_HPP

#include "core/data/ArrayList.hpp"
#include "core/math/Matrix.hpp"

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

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

        void pop() {
            assert(stack.getLength() > 0);
            stack.removeLast();
        }

        void push() {
            stack.add(peek());
        }

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

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

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

        void toString(BufferString& s) const {
            s.append(stack);
        }
    };
}

#endif