#include <string.h>
#include "glslUtil.h"

void guMatrixLoadIdentity(guMatrix4x4 *m)
{
    int i, j;
    for (i = 0; i < 4; i++)
    {
        for (j = 0; j < 4; j++)
        {
            (*m)[i][j] = (i == j) ? 1.0 : 0.0;
        }
    }
}

void guMatrixMult(guMatrix4x4 *m, guMatrix4x4 *a, guMatrix4x4 *b)
{
    guMatrix4x4 tmp;
    int i, j, p;
    if (m == a)
    {
        memcpy(&tmp, a, sizeof(tmp));
        a = &tmp;
    }
    for (i = 0; i < 4; i++)
    {
        for (j = 0; j < 4; j++)
        {
            float v = 0.0;
            for (p = 0; p < 4; p++)
            {
                v += (*a)[i][p] * (*b)[p][j];
            }
            (*m)[i][j] = v;
        }
    }
}

void guMatrixTranslate(guMatrix4x4 *m, float x, float y, float z)
{
    int i;
    for (i = 0; i < 4; i++)
    {
        (*m)[i][3] += (*m)[i][0] * x + (*m)[i][1] * y + (*m)[i][2] * z;
    }
}

void guMatrixScale(guMatrix4x4 *m, float x, float y, float z)
{
    int i;
    for (i = 0; i < 4; i++)
    {
        float *base = &(*m)[i][0];
        base[0] *= x;
        base[1] *= y;
        base[2] *= z;
    }
}

void guMatrixFrustum(guMatrix4x4 *m,
        float left, float right,
        float bottom, float top,
        float near, float far)
{
    int i, j;
    guMatrix4x4 mult;
    for (i = 0; i < 4; i++)
    {
        for (j = 0; j < 4; j++)
        {
            mult[i][j] = 0.0;
        }
    }
    mult[0][0] = 2 * near / (right - left);
    mult[0][2] = (right + left) / (right - left);
    mult[1][1] = 2 * near / (top - bottom);
    mult[1][2] = (top + bottom) / (top - bottom);
    mult[2][2] = - (far + near) / (far - near);
    mult[2][3] = - 2 * far * near / (far - near);
    mult[3][2] = -1.0;
    guMatrixMult(m, m, &mult);
}