#include <math.h>
#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++)
        {
            GLfloat v = 0.0;
            for (p = 0; p < 4; p++)
            {
                v += (*a)[i][p] * (*b)[p][j];
            }
            (*m)[i][j] = v;
        }
    }
}

void guMatrixTranslate(guMatrix4x4 *m, GLfloat x, GLfloat y, GLfloat 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, GLfloat x, GLfloat y, GLfloat z)
{
    int i;
    for (i = 0; i < 4; i++)
    {
        GLfloat *base = &(*m)[i][0];
        base[0] *= x;
        base[1] *= y;
        base[2] *= z;
    }
}

void guMatrixRotate(guMatrix4x4 *m, GLfloat angle,
        GLfloat x, GLfloat y, GLfloat z)
{
    GLfloat lx = x, ly = y, lz = z, p;
    p = lx*lx + ly*ly + lz*lz;
    if (p != 1.0)
    {
        GLfloat scale = sqrt(p);
        lx /= scale;
        ly /= scale;
        lz /= scale;
    }
    GLfloat c = cos(angle);
    GLfloat s = sin(angle);
    guMatrix4x4 mult;
    GLfloat oc = 1 - c;
    mult[0][0] = lx * lx * oc + c;
    mult[0][1] = lx * ly * oc - lz * s;
    mult[0][2] = lx * lz * oc + ly * s;
    mult[0][3] = 0.0;
    mult[1][0] = ly * lx * oc + lz * s;
    mult[1][1] = ly * ly * oc + c;
    mult[1][2] = ly * lz * oc - lx * s;
    mult[1][3] = 0.0;
    mult[2][0] = lx * lz * oc - ly * s;
    mult[2][1] = ly * lz * oc + lx * s;
    mult[2][2] = lz * lz * oc + c;
    mult[2][3] = 0.0;
    mult[3][0] = 0.0;
    mult[3][1] = 0.0;
    mult[3][2] = 0.0;
    mult[3][3] = 1.0;
    guMatrixMult(m, m, &mult);
}

void guMatrixFrustum(guMatrix4x4 *m,
        GLfloat left, GLfloat right,
        GLfloat bottom, GLfloat top,
        GLfloat near, GLfloat 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);
}

void guPerspective(guMatrix4x4 *m, GLfloat fovy, GLfloat aspect,
        GLfloat near, GLfloat far)
{
    int i, j;
    guMatrix4x4 mult;
    for (i = 0; i < 4; i++)
    {
        for (j = 0; j < 4; j++)
        {
            mult[i][j] = 0.0;
        }
    }
    GLfloat f = 1.0 / tan(fovy / 2.0);
    mult[0][0] = f / aspect;
    mult[1][1] = f;
    mult[2][2] = (far + near) / (near - far);
    mult[2][3] = (2 * far * near) / (near - far);
    mult[3][2] = -1.0;
    guMatrixMult(m, m, &mult);
}