#ifndef _MAT_H_
#define _MAT_H_
#include <iostream>
#include <math.h>
#include "Vec.h"
/**
 * @addtogroup math
 * @{
 * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
 * @version 2.0
 *
 * @section LICENSE
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details at
 * https://www.gnu.org/copyleft/gpl.html
 *
 * @section DESCRIPTION
 *
 * This file contains the Trajectory for a linear trajectory
 * The linear trajectory only uses the max speed and ignores
 * the acceleration parameter.
 * The movement is a strict hard acceleration and deacceration.
 *
 * The slowest joint is defining the speed of the other joints.
 * By that all joints start and stop the movement synchronously
 */

template <class T, unsigned SIZE> class Vec;

template <class T, unsigned SIZE> class Mat;
// 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;

typedef Mat<float, 4> MatCarthesian;


/**
 * Matrix class for class and simple data types
 * template with type and size of the vector
 *
 * @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
 * @copyright 2012 philipp schoenberger All rights reserved.
 * @license This project is released under the GNU Public License.
 */
template <class T, unsigned SIZE>
class Mat 
{
public:

    /**
     * standard constructor which is initialising all vales to zero
     */
    Mat<T, SIZE> ()
    {
        for (unsigned int x=0; x<SIZE; x++)
        {
            for (unsigned int y=0; y<SIZE; y++)
            {
                m_aatData[x][y] = T(0);
            }
        }
    }

    /**
     * default deconstructor which has to do nothing
     */
    ~Mat<T, SIZE> ()
    {
        // nothing to do here ...
    }

    /**
     * copy constructor for flat arrays of the simple type 
     *
     * @param srcData source data for the new matrix
     */
    Mat<T, SIZE> (const T srcData[SIZE][SIZE])
    {
        for (unsigned int x=0; x<SIZE; x++)
        {
            for (unsigned int y=0; y<SIZE; y++)
            {
                m_aatData[x][y] = srcData[x][y];
            }
        }
    }

    /**
     * constructor for an identity matrix from left top to right bottom
     *
     * @param initVal the value in each field beside the diagonal entries
     * @param eyeVal  the value on each filed exactly on the diagonal
     */
    Mat<T, SIZE> (const T initVal,const T eyeVal)
    {
        for (unsigned int x=0; x<SIZE; x++)
        {
            for (unsigned int y=0; y<SIZE; y++)
            {
                if (x == y)
                    m_aatData[x][y] = eyeVal;
                else
                    m_aatData[x][y] = initVal;
            }
        }
    }

    /**
     * constructor for an matrix with a defined value in each cell
     *
     * @param initVal the value for each field
     */
    Mat<T, SIZE> (const T initVal)
    {
        for (unsigned int x=0; x<SIZE; x++)
        {
            for (unsigned int y=0; y<SIZE; y++)
            {
                m_aatData[x][y] = initVal;
            }
        }
    }

    /**
     * copy constructor for matrix-matrix copy
     *
     * @param srcMat the value for each field
     */
    Mat<T, SIZE> (const Mat<T, SIZE> &srcMat)
    {
        for (unsigned int x=0; x<SIZE; x++)
        {
            for (unsigned int y=0; y<SIZE; y++)
            {
                m_aatData[x][y] = srcMat.m_aatData[x][y];
            }
        }
    }

    /**
     * operator for cell access and assignment
     *
     * @param x x axis position within the matrix
     * @param y y axis position within the matrix
     */
    T &operator () (unsigned x, unsigned y)
    {
        if (y>=SIZE)
            y = SIZE-1;
        if (x>=SIZE)
            x = SIZE-1;
        return m_aatData[x][y];
    }

    /**
     * operator for cell access without writing to it
     *
     * @param x x axis position within the matrix
     * @param y y axis position within the matrix
     */
    T operator () (unsigned x, unsigned y) const
    {
        if (x>=SIZE)
            x = SIZE-1;
        if (y>=SIZE)
            y = SIZE-1;
        return m_aatData[x][y];
    }

    /**
     * operator to add a simple type to each entry
     * within the array
     *
     * @param scalar the value to add
     * @return the new calculated matrix
     */
    Mat<T, SIZE> operator + (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int x=0; x<SIZE; x++) // row
            for (unsigned int y=0; y<SIZE; y++) // column
                buf(x,y) += m_aatData[x][y] + scalar;
        return buf;
    }

    /**
     * operator to subtract a simple type to each entry
     * within the array
     *
     * @param scalar the value to subtract
     * @return the new calculated matrix
     */
    Mat<T, SIZE> operator - (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int x=0; x<SIZE; x++) // row
            for (unsigned int y=0; y<SIZE; y++) // column
                buf(x,y) += m_aatData[x][y] - scalar;
        return buf;
    }

    /**
     * operator to multiply a simple type to each entry
     * within the array
     *
     * @param scalar the value to multiply the matrix cells
     * @return the new calculated matrix
     */
    Mat<T, SIZE> operator * (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                buf(row,column) += m_aatData[row][column] * scalar;
        return buf;
    }

    /**
     * operator to divide a simple type to each entry
     * within the array
     *
     * @param scalar the value to divide the matrix cells
     * @return the new calculated matrix
     */
    Mat<T, SIZE> operator / (const T &scalar)
    {
        Mat<T, SIZE> buf;
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                buf(row,column) += m_aatData[row][column] / scalar;
        return buf;
    }

    /**
     * operator to multiply tow different matrices 
     *
     * @param mat matrix to multiply 
     * @return the new calculated matrix
     */
    Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
    {
        Mat<T, SIZE> buf;
        for (unsigned int i=0; i<SIZE; i++) // row i
            for (unsigned int j=0; j<SIZE; j++) // column j
                for (unsigned int a=0; a<SIZE; a++) // a
                    buf(i,j) += m_aatData[i][a] * mat(a,j);
        return buf;
    }
    /**
     * This functions is used for the assigning operator.
     * for a simple array for the right hand side.
     * The array has to be column wise coded.
     *
     * @info the template type has to provide the abs function
     *
     * @param aatData the right hand side array
     * @return the new matrix 
     */
    Mat<T, SIZE> &operator = (const T aatData[SIZE*SIZE]) 
    {
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++) // column j
                m_aatData[row][column] = aatData[column+(row*4)];

        return (*this); // also an R-value in e.g.
                        // vec1  = vec2  +   (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
                        // L-Val = R-Val +       R-Val
                        //   "   =   "   + (L-Val = R-Val)
    }

    /**
     * This functions calculates the matrix containing only the 
     * absolute values of the current one.
     *
     * @info the template type has to provide the abs function
     *
     * @return the matrix with absolute values 
     */
    Mat<T,SIZE> abs()
    {
        Mat<T,SIZE> buf;
        for (unsigned int j=0; j<SIZE; j++) // column j
            for (unsigned int i=0; i<SIZE; i++) // row i
                buf.m_aatData[i][j] = abs(m_aatData[i][j]);
        return buf;
    }

    /**
     * This functions calculates the determinant of a matrix
     *
     * @return the determinant
     */
    T determinant()
    {
        T buf = 0;
        if (SIZE == 1)
            return m_aatData[0][0];
        if (SIZE == 2)
            return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];


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

            //std::cout <<"+("<< i << ", " << (i+1)%SIZE << ", " << (i+2)%SIZE << ") ";

            buf += multBuff;
            multBuff = m_aatData[0][i];
            multBuff *= m_aatData[1][(i+SIZE-1)%SIZE];
            multBuff *= m_aatData[2][(i+SIZE-2)%SIZE];

            //std::cout <<"-("<< i <<", " << (i+SIZE-1)%SIZE << ", " << (i+SIZE-2)%SIZE << ") ";

            buf -= multBuff;
        }
        return buf;
    }

    /**
     * This functions calculates the norm of the matrix.
     *
     * @info the Template type has to provide the sqrt and pow function
     * @return the normalisation factor
     */
    Mat<T,SIZE> norm()
    {
        T buf;
        for (unsigned int row=0; row < SIZE; row++)
            for (unsigned int column=0; column < SIZE; column++)
                buf +=  pow(abs(m_aatData[row][column]),2);
        sqrt(buf);
        return buf;
    }

    /**
     * This functions generates the transposed matrix for the 
     * current one.
     * @return the transposed matrix
     *
     * @see transpose
     * @see determinant
     */
    Mat<T,SIZE> transpose()
    {
        Mat<T, SIZE> buf;
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                buf.m_aatData[row][column] = m_aatData[row][column];
        return buf;
    }
    /**
     * This functions generates the inverse matrix for the 
     * current one. 
     * This is done via the determinant and the transposed.,
     * @return the inverted matrix
     *
     * @see transpose
     * @see determinant
     */
    Mat<T,SIZE> inv()
    {
        T det = determinant();

        if (det== 0)
        {
            return *this;
        }

        Mat<float,4> thisTransposed = this->transpose();
        thisTransposed/det;

        return thisTransposed;
    }
    /**
     * This functions copies the vales from the matrix to a flat list 
     * of simple types. The order will be row wise.
     * @param atData the flat destination array
     */
    void getData (T atData[SIZE*SIZE]) 
    {
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                atData[row+column*SIZE] = m_aatData[row][column];
    }
    /**
     * This functions copies the vales from a flat list of simple types
     * to the matrix. The order has to be rowwise.
     * @param atData the flat source array
     */
    void setData (const T atData[SIZE*SIZE]) 
    {
        for (unsigned int row=0; row<SIZE; row++)
            for (unsigned int column=0; column<SIZE; column++)
                m_aatData[row][column] = atData[row+column*SIZE];
    }

private:
    /// the private container for the matrix cells
    T m_aatData[SIZE][SIZE];
};


/**
 * This function is used for the standard output stream and converts
 * the Matrix to a string.
 *
 * @param output the outputstream which should be extended by the matrix print
 * @param mat    the matrix which should be printed to the output stream
 * @return the changed output stream to be capable of something like "cout<<"test" << matrix;"
 */
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;
}

/**
 * This function creates a rotation matrix for a x-y-z rotation
 *
 * @param x rotation for the x axis in [ degree ]
 * @param y rotation for the x axis in [ degree ]
 * @param x rotation for the x axis in [ degree ]
 * @return a homogeneous cartesian matrix
 */
static Mat<float, 4> getRotationMatrix(float x_angle,float y_angle, float z_angle)
{
    Mat<float,4> tempx,tempy,tempz;

    // create x rotation
    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}
                         };

    // create y rotation
    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}
                         };

    // create z rotation
    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}
                          };

    // combine all rotations to a matrix class
    tempx=temp_x;
    tempy=temp_y;
    tempz=temp_z;
    return tempz*tempy*tempx;
}
/**
 * @}
 */
#endif