#ifndef _MATH3D_H_
#define _MATH3D_H_

#include <GL/glew.h>
#include <GL/freeglut.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#define M3D_PI (3.14159265358979323846)
#define M3D_PI_DIV_180 (0.017453292519943296)
#define M3D_INV_PI_DIV_180 (57.2957795130823229)

#define m3dDegToRad(x) ((x)*M3D_PI_DIV_180)
#define m3dRadToDeg(x) ((x)*M3D_INV_PI_DIV_180)

typedef float M3DMatrix44f[16]; /* 4x4 matrix */
typedef float M3DMatrix33f[9];  /* 3x3 matrix */
typedef float M3DVector4f[4];   /* vector of 4 floats */
typedef float M3DVector3f[3];   /* vector of 3 floats */
typedef float M3DVector2f[2];   /* vector of 2 floats */

/* math function prototypes */
void m3dFindNormalf(M3DVector3f, const M3DVector3f, const M3DVector3f, 
                   const M3DVector3f);
void m3dLoadIdentity33f(M3DMatrix33f);
void m3dLoadIdentity44f(M3DMatrix44f);
void m3dGetPlaneEquationf(M3DVector4f, const M3DVector3f,
                         const M3DVector3f, const M3DVector3f);
void m3dRotationMatrix44f(M3DMatrix44f, float, float, float, float);
void m3dMakePlanarShadowMatrixf(M3DMatrix44f, const M3DVector4f, const M3DVector3f);
void m3dMatrixMultiply44f(M3DMatrix44f, const M3DMatrix44f, const M3DMatrix44f);

static inline void m3dCrossProductf(M3DVector3f result, const M3DVector3f u, 
                                   const M3DVector3f v)
{
    result[0] = u[1] * v[2] - v[1] * u[2];
    result[1] = -u[0] * v[2] + v[0] * u[2];
    result[2] = u[0] * v[1] - v[0] * u[1];
}

static inline void m3dTransformVector3f(M3DVector3f vOut, const M3DVector3f v, 
                                       const M3DMatrix44f m)
{
    vOut[0] = m[0] * v[0] + m[4] * v[1] + m[8] *  v[2] + m[12];
    vOut[1] = m[1] * v[0] + m[5] * v[1] + m[9] *  v[2] + m[13];
    vOut[2] = m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14];
}

static inline void m3dScaleVector3f(M3DVector3f v, float scale)
{
    v[0] *= scale;
    v[1] *= scale;
    v[2] *= scale;
}

static inline float m3dGetVectorLengthSquaredf(const M3DVector3f u)
{
    return (u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]);
}

static inline float m3dGetVectorLengthf(const M3DVector3f u)
{
    return (float) (sqrt((double) (m3dGetVectorLengthSquaredf(u))));
}

static inline void m3dNormalizeVectorf(M3DVector3f u)
{
    m3dScaleVector3f(u, 1.0f / m3dGetVectorLengthf(u));
}

static inline void m3dTranslateMatrix44f(M3DMatrix44f m, float x, float y, 
                                        float z)
{
    m[12] += x;
    m[13] += y;
    m[14] += z;
}

static inline void m3dScaleMatrix44f(M3DMatrix44f m, float x, float y, float z)
{
    m[0] *= x;
    m[5] *= y;
    m[10] *= z;
}

/* transpose / invert, only 4x4 matricies supported */
#define TRANSPOSE44(dst, src)                \
{                                            \
    int i, j;                                \
    for (j = 0; j < 4; j++)                  \
    {                                        \
        for (i = 0; i < 4; i++)              \
        {                                    \
            dst[(j*4)+i] = src[(i*4)+j];     \
        }                                    \
    }                                        \
}

static inline void m3dTransposeMatrix44f(M3DMatrix44f dst, 
                                        const M3DMatrix44f src)
{
    TRANSPOSE44(dst, src);
}

static inline void m3dCopyVector3f(M3DVector3f dst, const M3DVector3f src)
{
    memcpy(dst, src, sizeof(M3DVector3f));
}

#endif