#ifndef _SVRDATA_H_
#define _SVRDATA_H_

#include <new>
#include <iostream>
#include <pthread.h>
#include <queue>

#include "Singleton.h"
#include "Trajectroy.h"
#include "vec.h"
#include "friComm.h"
#include "friremote.h"

#define JOINT_NUMBER (LBR_MNJ)
typedef Vec<float, JOINT_NUMBER> VecJoint;
typedef Vec<float, FRI_CART_VEC> VecTorque;
typedef Mat<float, 4> MatCarthesian;

class SvrData: public Singleton
{
private:
    struct {
        VecJoint jointPos;
        MatCarthesian cartPos;
        VecTorque forceAndTorque;
        float jacobian[FRI_CART_VEC * JOINT_NUMBER];
    } messured;
    struct {
        VecJoint jointPos;
        MatCarthesian cartPos;
    } commanded;
    
    struct {
        struct {
            VecJoint velocity;
            VecJoint accelaration;
            VecJoint torque;
            VecJoint range;
        } max;
        float currentVelocityPercentage;
        float currentAccelerationPercentage;
    } robot;

    struct {
        float sampleTimeMs;
        bool cancel;
        bool done;
        std::queue< Trajectory<JOINT_NUMBER>* > list;
        enum TrajectoryType type;
    }trajectory;

    friRemote* friInst;

    pthread_mutex_t dataLock;

    /// private constructor, because the database is a singleton
    SvrData();

    static bool instanceFlag;
    static SvrData* single;
    
    
    void updateMessurementData(
        VecJoint newJointPos,
        MatCarthesian newCartPos,
        VecTorque newForceAndTorque
        );

public:
    ~SvrData(void);
    static SvrData* getInstance();
    
    friRemote* getFriRemote();

    void updateMessurement();

    void lock();
    void unlock();

    VecJoint getMeasuredJointPos();
    MatCarthesian getMeasuredCartPos();

    int getMessuredJacobian(float* data, size_t size);
    VecTorque getMessuredForceTorque();
    VecJoint getMessuredJointPos();
    MatCarthesian getMessuredCartPos();

    VecJoint getCommandedJointPos();
    void     setCommandedJointPos(VecJoint newJointPos);
    
    MatCarthesian getCommandedCartPos();
    void     setCommandedCartPos(MatCarthesian newCartPos);
    
    VecJoint getMaxTorque();
    VecJoint getMaxVelocity();
    VecJoint getMaxAcceleration();
    VecJoint getMaxRange();
    
    bool     checkJointRange(VecJoint vectorToCheck);

    VecJoint getRobotVelocity();
    float    getRobotVelocityPercentage();
    int      setRobotVelocityPercentage(float newVelocityPercentage);

    VecJoint getRobotAcceleration();
    float    getRobotAccelerationPercentage();
    int      setRobotAccelerationPercentage(float newAccelerationPercentage);

    unsigned int getJoints();

    // trajectory functions
    float getSampleTimeMs();
    int   setSampleTimeMs(float newSampleTime);

    bool getTrajectoryCancel();
    class Trajectory<JOINT_NUMBER>*  popTrajectory();
    int  pushTrajectory(class Trajectory<JOINT_NUMBER>* newFrajectory);
    bool cancelCurrentTrajectory();
    void trajectoryCancelDone();
    
    bool getTrajectoryDone();
    
    enum TrajectoryType getTrajectoryType();
    void setTrajectoryType(enum TrajectoryType newType);


    template <unsigned SIZE>
    class Trajectory<SIZE>* calculateTrajectory(
            VecJoint maxJointVelocity,
            VecJoint maxJointAcceleration,
            VecJoint jointStart,
            VecJoint jointEnd
            );
    int  calculateAndAddNewTrajectory(VecJoint jointEnd);
};
#endif