#ifndef _ROBOT_H_
#define _ROBOT_H_

#include "Vec.h"
#include "Mat.h"

#include "friComm.h"
#include "friremote.h"
#include "defines.h"
#include "Trajectroy.h"

typedef Vec<float, FRI_CART_VEC> VecTorque;
#define jointNumber LBR_MNJ
typedef Vec<float,jointNumber> VecJoint;
class Robot
{
    public:

        typedef Vec<float,jointNumber> 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 );
};

#endif