#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;
class Robot
{
    public:
        /// number of joints for the default robot
        static const unsigned int joints = LBR_MNJ;
        /// typdef for a vector witch can hold a value for each joint
        ///@TODO find a better solution for a general joint vector escpecial if the joint numbers could be varify for different robots:
        typedef Vec<float,joints> VecJoint;

    protected:
        VecJoint jointRangeMin;
        VecJoint jointRangeMax;
        VecJoint jointVelocity;
        VecJoint jointAcceleration;
        VecJoint jointTorque;

    public:

        /**
         * default constructor for initialising the robot constraints
         */
        Robot()
        {
            this->jointRangeMin = VecJoint(-180.0f);
            this->jointRangeMax = VecJoint(+180.0f);
            this->jointVelocity = VecJoint(+100.0f);
            this->jointAcceleration = VecJoint(+2.0f);
            this->jointTorque = VecJoint(10.0f);
        }

        /**
         * Returns the joint count of the robot.
         *
         * @retval joint count
         */
        int getJointCount(void)
        {
            return joints;
        }

        /**
         * Returns the minimum range for each joint of the robot
         *
         * @retval vector containing minimum joint angles [ degree ]
         */
        VecJoint getJointRangeMin(void)
        {
            return VecJoint(this->jointRangeMin);
        }

        /**
         * Returns the maximum range for each joint of the robot
         *
         * @retval vector containing maximum joint angles [ degree ]
         */
        VecJoint getJointRangeMax(void)
        {
            return VecJoint(this->jointRangeMax);
        }

        /**
         * Checks if the angles provided by the function parameter
         * does match the valid range of the robot.
         *
         * @retval true if all angles are matching the range or false otherwise
         */
        bool validateJointRange(VecJoint jointAngles)
        {
            for(unsigned int i = 0 ; i < joints ; ++i)
            {
                // angle to big
                if (jointAngles(i) > this->jointRangeMax(i))
                    return false;
                
                // angle to small
                if (jointAngles(i) < this->jointRangeMin(i))
                    return false;
            }
            return true;
        }

        /**
         * Returns the maximum velocity for each joint of the robot
         *
         * @retval vector containing maximum velocity for each joint [ degree/seconds ]
         */
        VecJoint getJointVelocity(void)
        {
            return VecJoint(this->jointVelocity);
        }

        /**
         * Returns the maximum acceleration for each joint of the robot
         *
         * @retval vector containing maximum acceleration for each joint [ degree/seconds^2 ]
         */
        VecJoint getJointAcceleration(void)
        {
            return VecJoint(this->jointAcceleration);
        }

        /**
         * Returns the maximum torque for each joint of the robot
         *
         * @retval vector containing maximum torque for each joint [ Nm ]
         */
        VecJoint getJointTorqueMax(void)
        {
            return VecJoint(this->jointTorque);
        }

        /**
         * Returns the david  hadenberg parameter for the requested joint
         *
         * @retval vector containing the 4 parameter
         */
        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;
        };

        /**
         * Calculates the joint angles for a given Position and arm configuration
         *
         * @param M      homogenious cartesian position
         * @param config the robot arm configuration ( elbow etc.)
         * @return the joint angles for the requested position
         */
        VecJoint backwardCalc(MatCarthesian M, struct RobotConfig config )
        {
            // unused
            (void) M;
            (void) config;

            return VecJoint(0.0f);
        }

        /**
         * Calculates the homogenious cartesian position for a given angle 
         * and robot arm configuration
         *
         * @param M      homogenious cartesian position
         * @param config the robot arm configuration ( elbow etc.)
         * @return the joint angles for the requested position
         */
        MatCarthesian forwardCalc(VecJoint j, struct RobotConfig config )
        {
            // unused
            (void) j;
            (void) config;

            return MatCarthesian(0.0f,1.0f);
        }
};

#endif