#ifndef _MAT_H_
#define _MAT_H_
#include <iostream>
#include "vec.h"

template <class T, unsigned SIZE> class CVector;

// template square matrix class for SIMPLE data types 
template <class T, unsigned SIZE> class Mat 
{
public:

    Mat<T, SIZE> ()
    {
        for (unsigned int j=0; j<SIZE; j++)
        {
            for (unsigned int i=0; i<SIZE; i++)
            {
                m_aatData[i][j] = T(0);
            }
        }
    }

    ~Mat<T, SIZE> ()
    {
        // nothing to do here ...
    }

    Mat<T, SIZE> (const T aatData[SIZE][SIZE])
    {
        for (unsigned int j=0; j<SIZE; j++)
        {
            for (unsigned int i=0; i<SIZE; i++)
            {
                m_aatData[i][j] = aatData[i][j];
            }
        }
    }

    Mat<T, SIZE> (const Mat<T, SIZE> &mat)
    {
        for (unsigned int j=0; j<SIZE; j++)
        {
            for (unsigned int i=0; i<SIZE; i++)
            {
                m_aatData[i][j] = mat.m_aatData[i][j];
            }
        }
    }

    T &operator () (unsigned i, unsigned j)
    {
        if (i>=SIZE)
            i = SIZE-1;
        if (j>=SIZE)
            j = SIZE-1;
        return m_aatData[i][j];
    }

    T operator () (unsigned i, unsigned j) const
    {
        if (i>=SIZE)
            i = SIZE-1;
        if (j>=SIZE)
            j = SIZE-1;
        return m_aatData[i][j];
    }

    // CMatrix<T, SIZE>operator + (const Matrix<T, SIZE> &mat)
    // CMatrix<T, SIZE>operator - (const Matrix<T, SIZE> &mat)
    // ...

    Mat<T, SIZE> operator + (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
            for (unsigned int j=0; j<SIZE; j++) // Spalte j
                buf(i,j) += m_aatData[i][j] + scalar;
        return buf;
    }
    Mat<T, SIZE> operator - (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
            for (unsigned int j=0; j<SIZE; j++) // Spalte j
                buf(i,j) += m_aatData[i][j] - scalar;
        return buf;
    }
    Mat<T, SIZE> operator * (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
            for (unsigned int j=0; j<SIZE; j++) // Spalte j
                buf(i,j) += m_aatData[i][j] * scalar;
        return buf;
    }
    Mat<T, SIZE> operator / (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
            for (unsigned int j=0; j<SIZE; j++) // Spalte j
                buf(i,j) += m_aatData[i][j] / scalar;
        return buf;
    }

    Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
            for (unsigned int j=0; j<SIZE; j++) // Spalte j
                for (unsigned int a=0; a<SIZE; a++) // a
                    buf(i,j) += m_aatData[i][a] * mat(a,j);
        return buf;
    }

#if 0
    Mat<T, SIZE> operator * (const Vec<T, SIZE> &vec)
    {
        Vec<T, SIZE> buf;
        for (unsigned int j=0; j<SIZE; j++) // SPALTE j
            for (unsigned int i=0; i<SIZE; i++) // ZEILE i
                buf(i) += m_aatData[i][j]*vec(j);
        return buf;
    }
#endif

    Mat<T,SIZE> determinant()
    {
        T buf;

        for (unsigned int j=0; j<SIZE; j++) // SPALTE j
        {
            unsigned int i = j;
            T multBuff;
            multBuff = m_aatData[i][j];
            multBuff *= m_aatData[(i+1)%SIZE][(i+1)%SIZE];
            multBuff *= m_aatData[(i+2)%SIZE][(i+2)%SIZE];
            buf += multBuff;
            multBuff = m_aatData[i][j];
            multBuff *= m_aatData[(i-1)%SIZE][(i-1)%SIZE];
            multBuff *= m_aatData[(i-2)%SIZE][(i-2)%SIZE];
            buf -= multBuff;
        }
    }

    Mat<T,SIZE> norm()
    {
        T buf;
        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
            for (unsigned int j=0; j<SIZE; j++) // Spalte j
                buf +=  pow(abs(m_aatData[i][j]),2);
        sqrt(buf);
        return buf;
    }

    Mat<T,SIZE> transpose()
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
            for (unsigned int j=0; j<SIZE; j++) // Spalte j
                buf(i,j) += m_aatData[j][i];
    }

    Mat<float, 4> inv()
    {
        Mat<float,4> invOut;

        long indxc[4], indxr[4], ipiv[4];
        long i, icol, irow, j, ir, ic;
        long big, dum, pivinv,temp, bb;

        ipiv[0] = -1;
        ipiv[1] = -1;
        ipiv[2] = -1;
        ipiv[3] = -1;

        invOut = *this;


        for (i =0; i < 4 ; i++) {
            big =0.0f;
            for (j =0; j < 4 ; j++) {
                if (ipiv[j] != 0) {
                    if (ipiv[0] == - 1) {
                        if ((bb = (float) fabs(invOut(j,0))) > big) {
                            big = bb;
                            irow = j;
                            icol = 0;
                            }
                    } else if (ipiv[0] > 0) {
                        goto end;
                    }

                    if (ipiv[1] == - 1) {
                        if ((bb = (float) fabs(invOut(j,1))) > big) {
                            big = bb;
                            irow = j;
                            icol = 1;
                        }
                    } else if (ipiv[1] > 0) {
                        goto end;
                    }

                    if (ipiv[2] == - 1) {
                        if ((bb = (float) fabs(invOut(j,2))) > big) {
                            big = bb;
                            irow = j;
                            icol = 2;
                        }
                    } else if (ipiv[2] > 0) {
                        goto end;
                    }

                    if (ipiv[3] == - 1) {
                        if ((bb = (float) fabs(invOut(j,3))) > big) {
                            big = bb;
                            irow = j;
                            icol = 3;
                        }
                    } else if (ipiv[3] > 0) {
                        goto end;
                    }
                }
            }
            ++(ipiv[icol]);

            if (irow != icol) {
                temp = invOut(irow,0);
                invOut(irow, 0) = invOut(icol, 0);
                invOut(icol, 0) = temp;

                temp = invOut(irow,1);
                invOut(irow, 1) = invOut(icol, 1);
                invOut(icol,1) = temp;

                temp = invOut(irow,2);
                invOut(irow,2) = invOut(icol,2);
                invOut(icol,2) = temp;

                temp = invOut(irow,3);
                invOut(irow,3) = invOut(icol,3);
                invOut(icol,3) = temp;
            }

            indxr[i] = irow;
            indxc[i] = icol;

            if (invOut(icol,icol) ==0.0) {
                goto end;
            }

            pivinv =1.0f / invOut(icol,icol);
            invOut(icol,icol) =1.0f;
            invOut(icol,0) *= pivinv;
            invOut(icol,1) *= pivinv;
            invOut(icol,2) *= pivinv;
            invOut(icol,3) *= pivinv;

            if (icol != 0) {
                dum = invOut(0,icol);
                invOut(0,icol) =0.0f;
                invOut(0,0) -= invOut(icol,0) * dum;
                invOut(0,1) -= invOut(icol,1) * dum;
                invOut(0,2) -= invOut(icol,2) * dum;
                invOut(0,3) -= invOut(icol,3) * dum;
            }

            if (icol != 1) {
                dum = invOut(1,icol);
                invOut(1,icol) =0.0f;
                invOut(1,0) -= invOut(icol,0) * dum;
                invOut(1,1) -= invOut(icol,1) * dum;
                invOut(1,2) -= invOut(icol,2) * dum;
                invOut(1,3) -= invOut(icol,3) * dum;
            }

            if (icol != 2) {
                dum = invOut(2,icol);
                invOut(2,icol) =0.0f;
                invOut(2,0) -= invOut(icol,0) * dum;
                invOut(2,1) -= invOut(icol,1) * dum;
                invOut(2,2) -= invOut(icol,2) * dum;
                invOut(2,3) -= invOut(icol,3) * dum;
            }

            if (icol != 3) {
                dum = invOut(3,icol);
                invOut(3,icol) =0.0f;
                invOut(3,0) -= invOut(icol,0) * dum;
                invOut(3,1) -= invOut(icol,1) * dum;
                invOut(3,2) -= invOut(icol,2) * dum;
                invOut(3,3) -= invOut(icol,3) * dum;
            }
        }
        if (indxr[3] != indxc[3]) {
            ir = indxr[3];
            ic = indxc[3];
            temp = invOut(0,ir);
            invOut(0,ir) = invOut(0,ic);
            invOut(0,ic) = temp;
            temp = invOut(1,ir);
            invOut(1,ir) = invOut(1,ic);
            invOut(1,ic) = temp;
            temp = invOut(2,ir);
            invOut(2,ir) = invOut(2,ic);
            invOut(2,ic) = temp;
            temp = invOut(3,ir);
            invOut(3,ir) = invOut(3,ic);
            invOut(3,ic) = temp;
        }
        if (indxr[2] != indxc[2]) {
            ir = indxr[2];
            ic = indxc[2];

            temp = invOut(0,ir);
            invOut(0,ir) = invOut(0,ic);
            invOut(0,ic) = temp;

            temp = invOut(1,ir);
            invOut(1,ir) = invOut(1,ic);
            invOut(1,ic) = temp;

            temp = invOut(2,ir);
            invOut(2,ir) = invOut(2,ic);
            invOut(2,ic) = temp;

            temp = invOut(3,ir);
            invOut(3,ir) = invOut(3,ic);
            invOut(3,ic) = temp;
        }
        if (indxr[1] != indxc[1]) {
            ir = indxr[1];
            ic = indxc[1];

            temp = invOut(0,ir);
            invOut(0,ir) = invOut(0,ic);
            invOut(0,ic) = temp;

            temp = invOut(1,ir);
            invOut(1,ir) = invOut(1,ic);
            invOut(1,ic) = temp;

            temp = invOut(2,ir);
            invOut(2,ir) = invOut(2,ic);
            invOut(2,ic) = temp;

            temp = invOut(3,ir);
            invOut(3,ir) = invOut(3,ic);
            invOut(3,ic) = temp;
        }
        if (indxr[0] != indxc[0]) {
            ir = indxr[0];
            ic = indxc[0];

            temp = invOut(0,ir);
            invOut(0,ir) = invOut(0,ic);
            invOut(0,ic) = temp;

            temp = invOut(1,ir);
            invOut(1,ir) = invOut(1,ic);
            invOut(1,ic) = temp;

            temp = invOut(2,ir);
            invOut(2,ir) = invOut(2,ic);
            invOut(2,ic) = temp;

            temp = invOut(3,ir);
            invOut(3,ir) = invOut(3,ic);
            invOut(3,ic) = temp;
        }
end:
        return invOut;
    }

private:
    T m_aatData[SIZE][SIZE];
};
// some common vector classes (abbr. names)
typedef Mat<float, 2> Mat2f;
typedef Mat<float, 3> Mat3f;
typedef Mat<float, 4> Mat4f;

typedef Mat<double, 2> Mat2d;
typedef Mat<double, 3> Mat3d;
typedef Mat<double, 4> Mat4d;


template <class T, unsigned SIZE>
static std::ostream& operator<< (std::ostream& output,const Mat<T,SIZE> &mat)
{
    output << "( ";
    for(unsigned int j =0; j< SIZE; ++j)
    {
        output << "( ";
        for(unsigned int i =0; i< SIZE; ++i)
        {
            output << mat(j,i);
            if (i<4-1)
                output << " , ";
            else
                output << " )";
        }
        if (j<SIZE-1)
            output << " ) ; ";
        else
            output << " ) )";
    }
    return output;
}
static Mat<float, 4> getRotationMatrix(float x_angle,float y_angle, float z_angle)
{
    Mat<float,4> tempx,tempy,tempz;
    float temp_x[4][4] = { { 1,            0,             0, 0},
                           { 0, cos(x_angle), -sin(x_angle), 0},
                           { 0, sin(x_angle),  cos(x_angle), 0},
                           { 0,            0,             0, 1}
                         };

    float temp_y[4][4] = { { cos(y_angle), 0, -sin(y_angle), 0},
                           {            0, 1,             0, 0},
                           { sin(y_angle), 0,  cos(y_angle), 0},
                           {            0, 0,             0, 1}
                         };
    float temp_z[4][4] = { { cos(z_angle), -sin(z_angle), 0, 0},
                           { sin(z_angle),  cos(z_angle), 0, 0},
                           {            0,             0, 1, 0},
                           {            0,             0, 0, 1}
                          };

    tempx=temp_x;
    tempy=temp_y;
    tempz=temp_z;
    return tempz*tempy*tempx;
}
#endif