#include "GLGXMath.h"
 
#define MAX_MATRIX_STACK_LEVELS 32
 
class MatrixStack
{
private:
    GLGXMATRIX stack[MAX_MATRIX_STACK_LEVELS];
    int currentLevel;
 
public:
    MatrixStack();
 
    void clear();
 
    const GLGXMATRIX *getTop() const;
 
    void loadIdentity();
 
    void loadMatrix(GLGXMATRIX *matrix);
 
    void multMatrix(GLGXMATRIX *matrix);    
    void multMatrixLocal(GLGXMATRIX *matrix);
 
    void pop();
    void push();
 
    void rotateAxis(GLGXVECTOR3 *vector, float angle);  
    void rotateAxisLocal(GLGXVECTOR3 *vector, float angle);
 
    void rotateYawPitchRoll(float yaw, float pitch, float roll);
    void rotateYawPitchRollLocal(float yaw, float pitch, float roll);
 
    void scale(float x, float y, float z);
    void scaleLocal(float x, float y, float z);
 
    void translate(float x, float y, float z);
    void translateLocal(float x, float y, float z); 
};

#import <assert.h>
#import "MatrixStack.cpp"
 
// Constructs the matrix and loads the identity matrix.
MatrixStack::MatrixStack()
{
    clear();
}
 
// Clears the stack and pops all matrices.
void MatrixStack::clear()
{
    currentLevel = 0;
    this->loadIdentity();
}
 
// Retrieves the current matrix at the top of the stack.
const GLGXMATRIX *MatrixStack::getTop() const
{
    return &stack[currentLevel];
}
 
// Loads identity in the current matrix.
void MatrixStack::loadIdentity()
{   
    GLGXMatrixIdentity(&stack[currentLevel]);
}
 
// Loads the specified matrix in the current matrix.
void MatrixStack::loadMatrix(const GLGXMATRIX *matrix)
{
    stack[currentLevel] = *matrix;
}
 
// Multiplies the current matrix with the specified matrix.
void MatrixStack::multMatrix(const GLGXMATRIX *matrix)
{
    stack[currentLevel] = stack[currentLevel] * (*matrix);
}
 
// Multiplies the specified matrix with the current matrix.
void MatrixStack::multMatrixLocal(const GLGXMATRIX *matrix)
{
    stack[currentLevel] = (*(GLGXMATRIX *)matrix) * stack[currentLevel];
}
 
// Removes the current matrix from the top of the stack.
void MatrixStack::pop()
{   
    if (currentLevel > 0)
    {
        currentLevel--;
    }
}
 
// Adds a matrix level to the stack, by duplicating the current matrix.
void MatrixStack::push()
{
    assert(currentLevel + 1 < MAX_MATRIX_STACK_LEVELS);
 
    stack[currentLevel + 1] = stack[currentLevel];
    currentLevel++;
}
 
// Rotates around an arbitrary vector axis. (world space)
void MatrixStack::rotateAxis(const GLGXVECTOR3 *vector, float angle)
{
    GLGXMATRIX tmp;
    GLGXMatrixRotationAxis(&tmp, (GLGXVECTOR3 *)vector, angle);
    stack[currentLevel] = stack[currentLevel] * tmp;
}
 
// Rotates around an arbitrary vector axis. (local space)
void MatrixStack::rotateAxisLocal(const GLGXVECTOR3 *vector, float angle)
{
    GLGXMATRIX tmp;
    GLGXMatrixRotationAxis(&tmp, (GLGXVECTOR3 *)vector, angle);
    stack[currentLevel] = tmp * stack[currentLevel];
}
 
// Rotates around an arbitrary axis. (world space)
void MatrixStack::rotateYawPitchRoll(float yaw, float pitch, float roll)
{
    GLGXMATRIX tmp;
    GLGXQUATERNION quaternion;
    GLGXQuaternionRotationYawPitchRoll(&quaternion, yaw, pitch, roll);
    GLGXMatrixRotationQuaternion(&tmp, &quaternion);
    stack[currentLevel] = stack[currentLevel] * tmp;
}
 
// Rotates around an arbitrary axis. (local space)
void MatrixStack::rotateYawPitchRollLocal(float yaw, float pitch, float roll)
{
    GLGXMATRIX tmp;
    GLGXQUATERNION quaternion;
    GLGXQuaternionRotationYawPitchRoll(&quaternion, yaw, pitch, roll);
    GLGXMatrixRotationQuaternion(&tmp, &quaternion);
    stack[currentLevel] = tmp * stack[currentLevel];
}
 
// Scale the current matrix. (world origin)
void MatrixStack::scale(float x, float y, float z)
{
    GLGXMATRIX tmp;
    GLGXMatrixScaling(&tmp, x, y, z);
    stack[currentLevel] = stack[currentLevel] * tmp;     
}
 
// Scale the current matrix. (local origin)
void MatrixStack::scaleLocal(float x, float y, float z)
{
    GLGXMATRIX tmp;
    GLGXMatrixScaling(&tmp, x, y, z);
    stack[currentLevel] = tmp * stack[currentLevel];     
}
 
// Translate the current matrix. (world origin)
void MatrixStack::translate(float x, float y, float z)
{
    GLGXMATRIX tmp;
    GLGXMatrixTranslation(&tmp, x, y, z);
    stack[currentLevel] = stack[currentLevel] * tmp;
}
 
// Translate the current matrix. (local origin)
void MatrixStack::translateLocal(float x, float y, float z)
{
    GLGXMATRIX tmp;
    GLGXMatrixTranslation(&tmp, x, y, z);
    stack[currentLevel] = tmp * stack[currentLevel];
}