#ifndef _SVRDATA_H_
#define _SVRDATA_H_

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

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


class SvrData: public Singleton
{
private:
    struct {
        float jointPos[LBR_MNJ];
        float cartPos[FRI_CART_FRM_DIM];
        float jacobian[FRI_CART_VEC * LBR_MNJ];
        float forceAndTorque[FRI_CART_VEC];
    } messured;
    struct {
        float jointPos[LBR_MNJ];
        float cartPos[FRI_CART_FRM_DIM];
        float pose[3][4]; //TODO what is this for
    } commanded;
    
    struct {
        struct {
            float velocity[LBR_MNJ];
            float accelaration[LBR_MNJ];
            float torque[LBR_MNJ];
            float range[LBR_MNJ];
        } max;
        unsigned int joints;
        float currentVelocity;
        float currentAcceleration;
    } robot;

    pthread_mutex_t dataLock;

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

    static bool instanceFlag;
    static SvrData* single;

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

    void lock();
    void unlock();

    int getMessuredJointPos(float* data, size_t size);
    Vec<float,LBR_MNJ> getMessuredJointPos();

    int getMessuredCartPos (float* data, size_t size);
    Mat<float,LBR_MNJ> getMessuredCartPos();

    int getMessuredJacobian(float* data, size_t size);

    int getMessuredForceTorque(float* data, size_t size);
    Vec<float,LBR_MNJ> getMessuredForceTorque();

    int getCommandedJointPos(float* data, size_t size);
    Vec<float,LBR_MNJ> getCommandedJointPos();

    int getCommandedCartPos (float* data, size_t size);
    Mat<float,LBR_MNJ> getCommandedCartPos();

    float getMaxTorque(unsigned int pos);
    int   getMaxTorque(float* data, size_t size);
    Vec<float,LBR_MNJ> getMaxTorque();

    float getMaxVelocity(unsigned int pos);
    int   getMaxVelocity(float* data, size_t size);
    Vec<float,LBR_MNJ> getMaxVelocity();

    float getMaxAcceleration(unsigned int pos);
    int   getMaxAcceleration(float* data, size_t size);
    Vec<float,LBR_MNJ> getMaxAcceleration();

    float getMaxRange(unsigned int pos);
    int   getMaxRange(float* data, size_t size);
    Vec<float,LBR_MNJ> getMaxRange();

    float getRobotVelocity();
    int   setRobotVelocity(float newVelocity);

    float getRobotAcceleration();
    int   setRobotAcceleration(float newVelocity);

    unsigned int getJoints();
};
#endif