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() {
            (void)stack.add(Matrix());
        }

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

        check_return Error push() {
            return 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();
            (void)stack.add(Matrix());
        }

        template<typename String>
        check_return Error toString(String& s) const {
            return s.append(stack);
        }
    };
}

#endif