#include <GL/gl.h>
#include <stdio.h>

#include "robot_matrix.h"

Cmatrix::Cmatrix()
{
    Zeros();
}

Cmatrix::Cmatrix(double e11, double e12, double e13, double e14, 
                 double e21, double e22, double e23, double e24, 
                 double e31, double e32, double e33, double e34, 
                 double e41, double e42, double e43, double e44)
{
    element[0][0] = e11;
    element[0][1] = e12;
    element[0][2] = e13;
    element[0][3] = e14;

    element[1][0] = e21;
    element[1][1] = e22;
    element[1][2] = e23;
    element[1][3] = e24;

    element[2][0] = e31;
    element[2][1] = e32;
    element[2][2] = e33;
    element[2][3] = e34;

    element[3][0] = e41;
    element[3][1] = e42;
    element[3][2] = e43;
    element[3][3] = e44;
}


Cmatrix::~Cmatrix()
{
}


Cmatrix Cmatrix::operator+ (const Cmatrix &m)
{
	Cmatrix mnew;

    for (int j=0; j<4; j++)
    {
        for (int i=0; i<4; i++)
        {
            mnew.element[i][j] = this->element[i][j] + m.element[i][j];
        }
    }

	return mnew;
}

Cmatrix Cmatrix::operator* (const Cmatrix &m)
{
    Cmatrix mnew;

    for (int j=0; j<4; j++)
    {
        for (int i=0; i<4; i++)
        {
            for (int k=0; k<4; k++)
            {
                mnew.element[i][j] += this->element[i][k] * m.element[k][j];
            }
        }
    }

	return mnew;
}

GLfloat* Cmatrix::GetGLmatrixPointer()
{
    GLfloat *m = new GLfloat[16];

    int k = 0;
    for (int j=0; j<4; j++)
    {
        for (int i=0; i<4; i++)
        {
            m[k++] = element[i][j];
        }
    }

    return m;
}


void Cmatrix::Zeros()
{
    for (int j=0; j<4; j++)
        for (int i=0; i<4; i++)
            element[i][j] = 0;
}

void Cmatrix::Ones()
{
    for (int j=0; j<4; j++)
        for (int i=0; i<4; i++)
            element[i][j] = 1;
}

void Cmatrix::Identity()
{
    for (int j=0; j<4; j++)
        for (int i=0; i<4; i++)
            if (i==j)
                element[i][j] = 1;
            else
                element[i][j] = 0;            
}

void Cmatrix::PrintMatrix()
{
    for (int i=0; i<4; i++)
        printf("%f   %f   %f   %f\n", element[i][0], element[i][1], element[i][2], element[i][3]);
    printf("\n");
}

void Cmatrix::PrintGLmatrix()
{
    GLfloat matrix[16]; 
    glGetFloatv (GL_MODELVIEW_MATRIX, matrix);

    for (int i=0; i<4; i++)
        printf("GlMatrix: %f   %f   %f   %f\n", matrix[i], matrix[i + 4], matrix[i + 8], matrix[i + 12]);
    printf("\n\n");
}
