#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"

#include "mat.h"
#include "vec.h"
#include "robot.h"

#define JOINT_NUMBER (LBR_MNJ)

class SvrData: public Singleton
{
private:
    struct {
        Robot::VecJoint jointPos;
        MatCarthesian cartPos;
        VecTorque forceAndTorque;
        float jacobian[FRI_CART_VEC * JOINT_NUMBER];
    } messured;
    struct {
        Robot::VecJoint jointPos;
        MatCarthesian cartPos;
    } commanded;
    
    struct {
        struct {
            Robot::VecJoint velocity;
            Robot::VecJoint accelaration;
            Robot::VecJoint torque;
            Robot::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(
        Robot::VecJoint newJointPos,
        MatCarthesian newCartPos,
        VecTorque newForceAndTorque
        );

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

    void updateMessurement();

    void lock();
    void unlock();

    Robot::VecJoint getMeasuredJointPos();
    MatCarthesian getMeasuredCartPos();

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

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

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

    Robot::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);

    class Trajectory<JOINT_NUMBER>* createTrajectory(Robot::VecJoint newJointPos);
    class Trajectory<JOINT_NUMBER>* createTrajectory(MatCarthesian newJointPos);

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