#ifndef _ROBOT_H_
#define _ROBOT_H_

#include "vec.h"
#include "mat.h"
#include "friComm.h"

typedef Vec<float, FRI_CART_VEC> VecTorque;

class Robot
{
    public :
        const static int joints = 7;
        typedef Vec<float,joints> VecJoint;

        static int      getJointCount(void)
        {
        }
        static VecJoint getJointRange(void)
        {
            return VecJoint(180.0f);
        }
        static VecJoint getJointVelocity(void)
        {
            return VecJoint(200.0f);
        }
        static VecJoint getJointAcceleration(void)
        {
            return VecJoint(10.0f);
        }

        static Vec<float, 4> getDhParameter(unsigned int joint)
        {
            // unused parameter
            (void) joint;
            return Vec<float,4>(0.0f);
        }
        
        struct RobotConfig
        {
            bool elbow;
            bool flip;
            bool j1os;
        };

        static VecJoint      backwardCalc(MatCarthesian, struct RobotConfig config );
        static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config );
};



#if 0
Mat4f vecToMat(float vec[12])
{
    Mat4f result;
    for (int i=0; i<3; i++) // ZEILE
    {
        for (int j=0; j<4; j++) // SPALTE
        {
            result(i,j) = vec[i*4+j];
        }
    }
    result(3,3)= 1.0f;
    return result;
}

float* matToVec(Mat4f mat)
{
    float* vec = new float[12];
    for (int j=0;j<3;j++)
    {
        for (int k=0;k<4;k++)
        {
            vec[j*4+k] = mat(j,k);
        }
    }
    return vec;
}

Mat4f getTranslation(double tx, double ty, double tz)
{
    Mat4f transl;
    transl(0,0) = 1;
    transl(1,1) = 1;
    transl(2,2) = 1;

    transl(0,3) = tx;
    transl(1,3) = ty;
    transl(2,3) = tz;
    transl(3,3) = 1;
    return transl;
}
#endif 

#endif