#ifndef _VEC_H_
#define _VEC_H_
#include <math.h>
#include <ostream>
#include <cmath>
#include "Mat.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;


/**
 * Vector 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 Vec 
{
public:

    /**
     * standard constructor
     */
    Vec<T,SIZE> ()  
    {   
        // initialize all elements with zero
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = T(0);
    }
    
    /**
     * construction with data value
     * Initialize the Vector completely to the given value
     *
     * @param tData initial value for the vector
     */
    Vec<T, SIZE> (const T tData) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = tData;
    }

    /**
     * construction with data array
     * Initialize the Vector completely for each field with array values 
     *
     * @param tData  array of initial values for the vector
     */
    Vec<T, SIZE> (const T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = atData[i];
    }

    /**
     * copy construction from an other vector
     * Initialize the Vector completely for each field like the given vector
     *
     * @param vec of initial values for the new vector
     */
    Vec<T, SIZE> (const Vec<T, SIZE> &vec) 
    {
        if (this==&vec)
            return; // nothing to do, it's me
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = vec.m_atData[i];
    }

    /**
     * destructor which has nothing 
     */
    ~Vec () 
    {
        // nothing to do here ...
    }

    /**
     * copy the values from the array to the Vector
     *
     * @param atData source array for the copy process
     */
    void setData (const T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = atData[i];
    }

    /**
     * copy the values from the vector to the array
     *
     * @param atData destination array for the copy process
     */
    void getData (T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            atData[i] = m_atData[i];
        
    }

    /**
     * returns the dimension of the vector 
     *
     * @retval dimension size 
     */
    unsigned getDimension () 
    {
        return SIZE;
    }


    /**
     * assignment operator between vectors
     * also capable of multi assignments 
     * e.g: 
     *
     * vec1    =       vec2      = vec3;
     * L-Value = R-Value/L-Value = R-Value
     *
     * @param vec source vector which should be copied
     * @retval returns the current 
     */
    Vec<T, SIZE> &operator = (const Vec<T, SIZE> &vec) 
    {
        /// check if the L and R values are the same 
        /// do nothing in this case
        if (this==&vec)
            return (*this); 

        /// not the same values so copy content data
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = vec.m_atData[i];

        // also an R-value in e.g
        // vec1    =       vec2      = vec3;
        // L-Value = R-Value/L-Value = R-Value
        return (*this); 
    }

    /**
     * assignment operator between simple types
     * also capable of multi assignments 
     * e.g: 
     *
     * vec1    =       vec2      = vec3;
     * L-Value = R-Value/L-Value = R-Value
     *
     * @param atData the source data array which should be copied
     * @retval returns the current 
     */
    Vec<T, SIZE> &operator = (const T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++) // not me, so L-Value action: copy data
            m_atData[i] = atData[i];

        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 requests the cell with a defined index
     * usage of operator:
     * vec(i) = var;    // 0 <= i <= SIZE-1
     * var = vec(i);
     * vec1(i) = vec2(j);
     *
     * @param i the index of the vector
     * @retval returns the cell with the provided index
     */
    T &operator () (unsigned i) 
    {
        if (i>=SIZE)
            i = SIZE-1; // !!! operator clips index ...
        return m_atData[i];         // ... so we can safely return a reference
    }

    /**
     * This functions requests the cell with a defined index
     * usage of operator:
     * vec(i) = var;    // 0 <= i <= SIZE-1
     * var = vec(i);
     * vec1(i) = vec2(j);
     *
     * @param i the index of the vector
     * @retval returns the cell with the provided index
     */
    T operator () (unsigned i) const 
    {
        if (i>=SIZE)
            i = SIZE-1;
        return m_atData[i];
    }

    /**
     * operator to adds the vector by 
     * an other vector and reasign it.
     *
     * @param vec the vector to add to the current instance
     */
    void operator += (const Vec<T, SIZE> &vec) 
    {
        for (int i=0; i<SIZE; i++)
            m_atData[i] += vec.m_atData[i];
    }

    /**
     * operator to adds the vector by 
     * an other vector.
     *
     * @param vec the vector to add to the current instance
     * @return the calculated vector
     */
    Vec<T, SIZE> operator + (const Vec<T, SIZE> &vec) 
    {
        Vec<T, SIZE> buf (m_atData);
        for (unsigned int i=0; i<SIZE; i++)
            buf.m_atData[i] += vec.m_atData[i];
        return buf;
    }

    /**
     * operator to subtract the vector by 
     * an other vector.and reasign it
     *
     * @param vec the vector to subtract to the current instance
     */
    void operator -= (const Vec<T, SIZE> &vec) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] -= vec.m_atData[i];
    }

    /**
     * operator to subtract the vector by 
     * an other vector.
     *
     * @param vec the vector to subtract to the current instance
     * @return the calculated vector
     */
    Vec<T, SIZE> operator - (const Vec<T, SIZE> &vec) 
    {
        Vec<T, SIZE> buf (m_atData);
        for (unsigned int i=0; i<SIZE; i++)
            buf.m_atData[i] -= vec.m_atData[i];
        return buf;
    }

    /**
     * operator to calculate the negative vector
     * of the current one
     *
     * @return the negative vector
     */
    Vec<T, SIZE> operator - () 
    {
        T atBuffer[SIZE];
        for (int i=0; i<SIZE; i++)
            atBuffer[i] = -m_atData[i];
        return Vec<T, SIZE> (atBuffer);
    }

    /**
     * operator to multiply the vector by 
     * an other vector.
     *
     * @param vec the vector to multiply to the current instance
     * @return the vector product
     */
    T operator * (const Vec<T, SIZE> & vec) 
    {
        T dp = T(0);
        for (int i=0; i<SIZE; i++)
            dp += m_atData[i]*vec.m_atData[i];
        return dp;
    }

    /**
     * operator to multiply the vector by the simple type
     * provided and reasign it.
     *
     * @param tscale the simple type
     */
    void operator *= (T tScale) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] *= tScale;
    }

    /**
     * operator to multiply the vector by the simple type
     * provided 
     *
     * @param tscale the simple type
     * @return the new calculated matrix
     */
    Vec<T, SIZE> operator * (T tScale) 
    {
        Vec<T, SIZE> vec;
        for (unsigned int i=0; i<SIZE; i++)
            vec(i) = m_atData[i]*tScale;
        return vec;
    }

    /**
     * operator to divide the vector by the simple type
     * provided 
     *
     * @param tscale the simple type
     * @return the new calculated matrix
     */
    Vec<T, SIZE> operator / (T tScale) 
    {
        Vec<T, SIZE> vec;
        for (unsigned int i=0; i<SIZE; i++)
            vec(i) = m_atData[i]/tScale;
        return vec;
    }

    /**
     * This functions returns from the current and 
     * the provided vector the vector division
     * for each cell separately
     *
     * @info the operator/ function has to be implemented for the template type
     *
     * @return the vector containing the cell wise divisor
     */
    Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)
    {
        Vec<T, SIZE> vec;
        for (unsigned int j=0; j<SIZE; j++)
            for (unsigned int i=0; i<SIZE; i++)
               vec(j) += m_atData[i]*mat(i,j);
        return vec;
    }

    /**
     * This functions returns from the current and 
     * the provided vector the vector division
     * for each cell separately
     *
     * @info the operator/ function has to be implemented for the template type
     *
     * @pram vec the vector to divide the current vector
     * @return the vector containing the cell wise divisor
     */
    Vec<T, SIZE> celldivide (const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = m_atData[i]/vec.m_atData[i];
        return buff;
    }

    /**
     * This functions returns from the current and 
     * the provided vector the vector multiplication 
     * for each cell separately
     *
     * @info the operator* function has to be implemented for the template type
     *
     * @pram vec the vector to multiply the current vector
     * @return the vector containing the cell wise product
     */
    Vec<T, SIZE> cellmultiply (const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = m_atData[i]*vec.m_atData[i];
        return buff;
    }

    /**
     * This functions returns from the current and 
     * the provided vector the minimum for each cell
     *
     * @info the min function has to be implemented for the template type
     *
     * @return the vector containing the minimum cells in each dimension
     */
    Vec<T, SIZE> cellmin(const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::min(m_atData[i],vec.m_atData[i]);
        return buff;
    }

    /**
     * This functions returns from the current and 
     * the provided vector the maximum for each cell
     *
     * @info the max function has to be implemented for the template type
     *
     * @return the vector containing the maximum cells in each dimension
     */
    Vec<T, SIZE> cellmax(const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::max(m_atData[i],vec.m_atData[i]);
        return buff;
    }

    /**
     * This functions calculates the absolute value
     * for each cell.
     *
     * @info the abs function has to be implemented for the template type
     *
     * @return the vector containing the absolute values
     */
    Vec<T, SIZE> abs ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::abs(m_atData[i]);
        return buff;
    }
    
    /**
     * This functions calculates the square root
     * for each cell.
     *
     * @info the sqrt function has to be implemented for the template type
     *
     * @return the vector containing the square root values
     */
    Vec<T, SIZE> sqrt()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::sqrt(m_atData[i]);
        return buff;
    }

    /**
     * This functions calculates the floor
     * for each cell.
     *
     * @info the floor function has to be implemented for the template type
     *
     * @return the vector containing the floor values
     */
    Vec<T, SIZE> floor ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = floor(m_atData[i]);
        return buff;
    }
    
    /**
     * This functions calculates the ceil
     * for each cell.
     *
     * @info the ceil function has to be implemented for the template type
     *
     * @return the vector containing the ceil values
     */
    Vec<T, SIZE> ceil ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::ceil(m_atData[i]);
        return buff;
    }

    /**
     * This functions is here for the float simple type
     * to calculate the sgn
     *
     * @param x the float value to check
     * @return the sign of the provided float
     */
    float sgn(float x) 
    { 
        if ( x == 0 ) 
            return 0.0f; 

        if ( x > 0)
            return 1.0f;
        else
            return -1.0f; 
    }

    /**
     * This functions returns the vector containing the 
     * sign for each cell.
     *
     * @info the Template type has to provide the sgn function
     * @return the vector with all signs
     */
    Vec<T, SIZE> sgn ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = sgn(m_atData[i]);
        return buff;
    }
    
    /**
     * This functions returns the maximum cell of the vector.
     *
     * @info the Template type has to provide the operator>
     * @return the maximum cell
     */
    T max ()
    {
        T retval = m_atData[0];

        // this * this
        for (int i=0; i<SIZE; i++)
            if (retval < m_atData[i])
                retval =  m_atData[i];
        return retval;
    }
    
    /**
     * This functions returns the minimum cell of the vector.
     *
     * @info the Template type has to provide the operator>
     * @return the minimum cell
     */
    T min ()
    {
        T retval = m_atData[0];

        // this * this
        for (int i=0; i<SIZE; i++)
            if (retval > m_atData[i])
                retval =  m_atData[i];
        return retval;
    }

    /**
     * This functions calculates the norm of the vector
     *
     * @info the Template type has to provide the sqrt function
     * @return the normalisation factor
     */
    T norm ()
    {
        T retval = 0;

        // this * this
        for (int i=0; i<SIZE; i++)
            retval += m_atData[i] * m_atData[i];
        sqrt(retval);
        return retval;
    }


    /**
     * This functions calculates the cross product of 2 vectors.
     * 
     * @param vec the 2th vector for the cross product
     * @return the cross product
     */
    Vec<T, SIZE> x(const Vec<T, SIZE> &vec)
    {
        T temp[SIZE];
        temp[0] = m_atData[1]*vec(2) - m_atData[2]*vec(1);
        temp[1] = m_atData[2]*vec(0) - m_atData[0]*vec(2);
        temp[2] = m_atData[0]*vec(1) - m_atData[1]*vec(0);
        temp[3] = vec(3);
        return Vec<T, SIZE> (temp);
    }

private:

    T m_atData[SIZE];

}; // class Vec


// some common vector classes (abbr. names)
typedef Vec<float, 2> Vec2f;
typedef Vec<float, 3> Vec3f;
typedef Vec<float, 4> Vec4f;

typedef Vec<double, 2> Vec2d;
typedef Vec<double, 3> Vec3d;
typedef Vec<double, 4> Vec4d;

typedef Vec<int, 2> Vec2i;
typedef Vec<int, 3> Vec3i;
typedef Vec<int, 4> Vec4i;

typedef Vec<bool, 2> Vec2b;
typedef Vec<bool, 3> Vec3b;
typedef Vec<bool, 4> Vec4b;


/**
 * This function is used for the standard output stream and converts
 * the vector to a string.
 *
 * @param output the outputstream which should be extended by the vector
 * @param vec    the vector 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 Vec<T,SIZE> &vec)
{
    output << "( ";
    for(unsigned int i = 0 ; i< SIZE; ++i)
    {
        output << vec(i);
        if (i<SIZE-1)
            output << " , ";
        else
            output << " )";
    }
    return output;
}

/**
 * @}
 */
#endif