#ifndef MATRIX_H
#define MATRIX_H

#include "vec3.h"

class CMatrix4d // Uses the OPENGL storing system!! So column over row
{
public:
    // Data
    float m[16];

    CMatrix4d( )
    {
        LoadIdentity();
    }

    inline void LoadIdentity()
    {
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                if (i == j)
                    m[i * 4 + j] = 1;
                else
                    m[i * 4 + j] = 0;
    }

	inline void Clear()
	{
        for (int i = 0; i < 16; i++)
            m[i] = 0; 
	}

    inline void Copy(const CMatrix4d& source)
    {
        for (int i = 0; i < 16; i++)
            m[i] = source.m[i];

    }


    inline CMatrix4d operator+ (const CMatrix4d& m1) const // Add
    {
        CMatrix4d result;

        for (int i = 0; i < 16; i++)
            result.m[i] = m[i] + m1.m[i];
    }

    inline CMatrix4d operator- (const CMatrix4d& m1) const // Substract
    {
        CMatrix4d result;

        for (int i = 0; i < 16; i++)
            result.m[i] = m[i] - m1.m[i];
    }

 /*   inline CVec3 operator* (const CVec3& vec) const
    {
        CVec3 result;

        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                //for (int k = 0; k < 3; k++)
                result[i] += m[i + j * 4] * vec[j];

        return result;

    }*/

    inline CMatrix4d& operator* (const CMatrix4d m1) const // Multiply
    {

        //	for (int i = 0; i < 4; i++)
        //		for (int j = 0; j < 4; j++)

		CMatrix4d result;
		result.Clear();
		float X[4][4][4];

		for(int i=0;i<4;i++)
		{     
          for(int j=0;j<4;j++)
          {     
               for(int k=0;k<4;k++)
               {
               X[i][j][k] = m[i * 4 + k] * m1.m[k * 4 + j];
               result.m[i * 4 + j] += X[i][j][k];
               }
          }
		}
		  return result;

     /*   CMatrix4d result;
        for (int r=0;r<4;r++)
        {
            for (int c=0;c<4;c++)
            {
                for (int i=0;i<4;i++)
                {
                    result.m[r * 4 + c] += m[r * 4 + i] * m1.m[i * 4 + c];
                }
            }
        }

        return result;*/

    }
	

    inline void Multiply (const CMatrix4d m1) // Multiply
    {
		float X[4][4][4] = {0};

		for(int i=0;i<4;i++)
		{     
          for(int j=0;j<4;j++)
          {     
               for(int k=0;k<4;k++)
               {
               X[i][j][k] = m[i * 4 + k] * m1.m[k * 4 + j];
               }
          }
		}


		Clear();

		for(int i=0;i<4;i++)   
          for(int j=0;j<4;j++)   
               for(int k=0;k<4;k++)
					m[i * 4 + j] += X[i][j][k];

		
		/*for (int r=0;r<4;r++)
        {
            for (int c=0;c<4;c++)
            {
                for (int i=0;i<4;i++)
                {
                    m[r * 4 + c] += m[r * 4 +i] * m1.m[i * 4 + c];
                }
            }
        }*/

    }


    inline void Translate(float x, float y, float z)
    {
        CMatrix4d trans;

		trans.LoadIdentity();
        trans.m[12] = x;
        trans.m[13] = y;
        trans.m[14] = z;

        Multiply(trans);
    }

    inline void Rotate(float x, float y, float z) // x, y and z are rotations in radians
    {
        CMatrix4d rot, rotfinal;

        rotfinal.LoadIdentity();

        // X-axis rotation
        rot.LoadIdentity();
        rot.m[5] = cosf(x);
        rot.m[6] = -sinf(x);
        rot.m[9] = sinf(x);
        rot.m[10] = cosf(x);
        rotfinal = rotfinal * rot;

        // Y-axis rotation
        rot.LoadIdentity();
        rot.m[0] = cosf(y);
        rot.m[2] = sinf(y);
        rot.m[8] = -sinf(y);
        rot.m[10] = cosf(y);
        rotfinal = rotfinal * rot;

        // Z-axis rotation
        rot.LoadIdentity();
        rot.m[0] = cosf(z);
        rot.m[1] = sinf(z);
        rot.m[4] = sinf(z);
        rot.m[5] = cosf(z);
        rotfinal = rotfinal * rot;

		CVec3 pos = this->GetPos();
		float k = this->m[15];
	/*	this->m[12] = 0;
		this->m[13] = 0;
		this->m[14] = 0;
		this->m[15] = 1;*/
        Multiply(rotfinal);

		this->m[12] = pos[0];
		this->m[13] = pos[1];
		this->m[14] = pos[2];
		this->m[15] = k;

    }

    CVec3 ToWorld(const CVec3 &p) const
    {
        CVec3 result;

        result.x = (p.x * m[0] + p.y * m[4] + p.z * m[8]  + m[12]);
        result.y = (p.x * m[1] + p.y * m[5] + p.z * m[9]  + m[13]);
        result.z = (p.x * m[2] + p.y * m[6] + p.z * m[10] + m[14]);

        return result;
    }

	 CVec4 ToWorld(const CVec4 &p) const
    {
        CVec4 result;

        result.x = (p.x * m[0] + p.y * m[4] + p.z * m[8]  + p.w * m[12]);
        result.y = (p.x * m[1] + p.y * m[5] + p.z * m[9]  + p.w * m[13]);
        result.z = (p.x * m[2] + p.y * m[6] + p.z * m[10] + p.w * m[14]);
		result.w = (p.x * m[3] + p.y * m[7] + p.z * m[11] + p.w * m[15]);

        return result;
    }

    inline void Inverse()
    {

        float data[3 * 4 * 4];
        int M = 4;

        for (int i=0; i<M;i++)
        {
            for (int j=0; j<2*M; j++)
            {
                if (j<M)
                {
                    data[i*2*M+j] = m[i*M+j];
                }
                else
                {
                    data[i*2*M+j] = 0;
                }

                if (j==i+M) data[i*2*M+j] =1.0;
            }
        }


        for (int ii=0; ii<M-1; ii++)
        {
            for (int kk=ii+1; kk < M; kk++)
            {
                for (int jj=2*M-1; jj>=ii; jj--)
                {
                    data[kk*2*M+jj] -= data[ii*2*M+jj] *
                                       (data[kk*2*M+ii]/data[ii*2*M+ii]);
                }
            }
        }

        //-------------------------------------

        for (int iii=M-1; iii>0; iii--)
        {
            for (int kkk=iii-1; kkk >=0; kkk--)
            {
                for (int jjj=2*M-1; jjj>=iii; jjj--)
                {
                    data[kkk*2*M+jjj] -= data[iii*2*M+jjj] *
                                         (data[kkk*2*M+iii]/data[iii*2*M+iii]);
                }
            }
        }

        for (int iiii=0; iiii<M; iiii++)
        {
            for (int jjjj=2*M-1; jjjj >=0; jjjj--)
            {
                data[iiii*2*M+jjjj] = data[iiii*2*M+jjjj]/ data[iiii*2*M+iiii];
            }
        }

        for (int s = 0; s<M; s++)
            for (int r=0; r<M; r++)
            {
                m[s*M+r] = data[s*2*M+r+M];
            }
        //  return I;
        //---------------------------------
    }

	// from http://www.experts-exchange.com/Programming/Programming_Languages/Cplusplus/Q_21651713.html

    CVec3 ToLocal(const CVec3 &p)
    {
        CVec3 result;
        CMatrix4d inverse;
        inverse.Copy(*this);
        inverse.Inverse();

        result.x = (p.x * inverse.m[0] + p.y * inverse.m[4] + p.z * inverse.m[8]  + inverse.m[12]);
        result.y = (p.x * inverse.m[1] + p.y * inverse.m[5] + p.z * inverse.m[9]  + inverse.m[13]);
        result.z = (p.x * inverse.m[2] + p.y * inverse.m[6] + p.z * inverse.m[10] + inverse.m[14]);

        return result;
    }

	CVec4 ToLocal(const CVec4 &p)
    {
        CVec4 result;
        CMatrix4d inverse;
        inverse.Copy(*this);
        inverse.Inverse();

        result.x = (p.x * inverse.m[0] + p.y * inverse.m[4] + p.z * inverse.m[8]  + p.w * inverse.m[12]);
        result.y = (p.x * inverse.m[1] + p.y * inverse.m[5] + p.z * inverse.m[9]  + p.w * inverse.m[13]);
        result.z = (p.x * inverse.m[2] + p.y * inverse.m[6] + p.z * inverse.m[10] + p.w * inverse.m[14]);
		result.w = (p.x * inverse.m[3] + p.y * inverse.m[7] + p.z * inverse.m[11] + p.w * inverse.m[15]);

        return result;
    }


	void LookAt(const CVec3 p, CVec3 Up)
	{		
		CVec3 Forward;		
		Forward.x = p.x - m[12];
		Forward.y = p.y - m[13];
		Forward.z = p.z - m[14];

		Forward.Normalize();
		Up.Normalize();
	
		CVec3 Right = Forward.Cross(Up);
		Right.Normalize();

		CVec3 Up2 = Right.Cross(Forward);
		Up2.Normalize();

		if (Forward.x == 0 && Forward.z == Forward.x) // degenerate case
			Up2.Set(- Forward.y, 0, 0);

		m[0] = Right.x;
		m[1] = Right.y;
		m[2] = Right.z;

		m[4] = Up2.x;
		m[5] = Up2.y;
		m[6] = Up2.z;

		m[8] = Forward.x;
		m[9] = Forward.y;
		m[10] = Forward.z;
	}

	CVec3 GetPos()
	{
		return CVec3(m[12], m[13], m[14]);
	}



};

#endif