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<int N>
    class MatrixStack final {
        ArrayList<Matrix, N> stack;

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

        check_return Error pop() {
            if(stack.getLength() <= 1) {
                return ErrorCode::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<typename String>
        check_return Error toString(String& s) const {
            return s.append(stack);
        }
    };
}

#endif