#include <pthread.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include "SvrData.h"

bool SvrData::instanceFlag = false;
SvrData* SvrData::single = 0;

SvrData::SvrData(void)
{

    // these are the default values for the robot 
    double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0};
    double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0};
    double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0};
    double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0};

    robot.currentVelocity = 20;
    robot.currentAcceleration = 10;
    robot.joints = LBR_MNJ;

    for (unsigned int i = 0; i < robot.joints; ++i)
    {
        robot.max.velocity[i] = maxVelJnt[i];
        robot.max.accelaration[i] = maxAccJnt[i];
        robot.max.torque[i] = maxTrqJnt[i];
        robot.max.range[i] = maxRangeJnt[i];
    }

    // init mutex for database
    if (pthread_mutex_init(&dataLock, NULL) != 0)
        printf("mutex init failed\n");
}

SvrData::~SvrData()
{
    pthread_mutex_destroy(&dataLock);
}

SvrData* SvrData::getInstance ()
{
    if (instanceFlag)
        return single;

    single = new SvrData();
    instanceFlag = true;
    return single;
}


void SvrData::lock()
{
    pthread_mutex_lock(&dataLock);
}

void SvrData::unlock()
{
    pthread_mutex_unlock(&dataLock);
}

int SvrData::getMessuredJointPos(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(messured.jointPos))
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,messured.jointPos,size);
    pthread_mutex_unlock(&dataLock);
    return 0;
}

Vec<float,LBR_MNJ> SvrData::getMessuredJointPos()
{
    Vec<float,LBR_MNJ> buf ;
    pthread_mutex_lock(&dataLock);
    buf = messured.jointPos;
    pthread_mutex_unlock(&dataLock);
    return buf;
}

int SvrData::getMessuredCartPos (float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(messured.cartPos))
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,messured.cartPos,size);
    pthread_mutex_unlock(&dataLock);
    return 0 ;
}

Mat<float,LBR_MNJ> SvrData::getMessuredCartPos()
{
    Mat<float,LBR_MNJ> buf ;
    pthread_mutex_lock(&dataLock);
    buf = messured.cartPos;
    pthread_mutex_unlock(&dataLock);
    return buf;
}

int SvrData::getMessuredJacobian(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(messured.jacobian))
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,messured.jacobian,size);
    pthread_mutex_unlock(&dataLock);
    return 0;
}

int SvrData::getMessuredForceTorque(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(messured.forceAndTorque))
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,messured.forceAndTorque,size);
    pthread_mutex_unlock(&dataLock);
    return 0;
}

int SvrData::getCommandedJointPos(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(commanded.jointPos))
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,commanded.jointPos,size);
    pthread_mutex_unlock(&dataLock);
    
    return 0;
}
Vec<float, LBR_MNJ> SvrData::getCommandedJointPos()
{
    Vec<float, LBR_MNJ> buff;
    pthread_mutex_lock(&dataLock);
    buff = commanded.jointPos;
    pthread_mutex_unlock(&dataLock);
    return buff;
}
int SvrData::getCommandedCartPos (float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(commanded.cartPos))
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,commanded.cartPos,size);
    pthread_mutex_unlock(&dataLock);

    return 0;
}
Mat<float, LBR_MNJ> SvrData::getCommandedCartPos ()
{
    Mat<float, LBR_MNJ> buff;
    pthread_mutex_lock(&dataLock);
    buff = commanded.cartPos;
    pthread_mutex_unlock(&dataLock);
    return buff;
}

float SvrData::getMaxTorque(unsigned int pos)
{
    float retval = -1.0f;
    if (pos >= robot.joints)
        return retval;

    pthread_mutex_lock(&dataLock);
    retval = robot.max.torque[pos-1];
    pthread_mutex_unlock(&dataLock);
    return retval;
}

int   SvrData::getMaxTorque(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(float)* robot.joints)
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,robot.max.torque,size);
    pthread_mutex_unlock(&dataLock);
    return 0;
}

float SvrData::getMaxAcceleration(unsigned int pos)
{
    float retval = -1.0f;
    if (pos >= robot.joints)
        return retval;

    pthread_mutex_lock(&dataLock);
    retval = robot.max.accelaration[pos-1];
    pthread_mutex_unlock(&dataLock);
    return retval;
}
int   SvrData::getMaxAcceleration(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(float)* robot.joints)
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,robot.max.accelaration,size);
    pthread_mutex_unlock(&dataLock);
    return 0;
}
Vec<float,LBR_MNJ>   SvrData::getMaxAcceleration()
{   
    Vec<float,LBR_MNJ> buff;
    pthread_mutex_lock(&dataLock);
    buff = robot.max.accelaration;
    pthread_mutex_unlock(&dataLock);
    return buff;
}
float SvrData::getMaxVelocity(unsigned int pos)
{
    float retval = -1.0f;
    if (pos >= robot.joints)
        return retval;

    pthread_mutex_lock(&dataLock);
    retval = robot.max.velocity[pos-1];
    pthread_mutex_unlock(&dataLock);
    return retval;
}
int   SvrData::getMaxVelocity(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(float)* robot.joints)
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,robot.max.velocity,size);
    pthread_mutex_unlock(&dataLock);
    return 0;
}
Vec<float,LBR_MNJ>   SvrData::getMaxVelocity()
{   
    Vec<float,LBR_MNJ> buff;
    pthread_mutex_lock(&dataLock);
    buff = robot.max.velocity;
    pthread_mutex_unlock(&dataLock);
    return buff;
}

float SvrData::getMaxRange(unsigned int pos)
{
    float retval = -1.0f;
    if (pos >= robot.joints)
        return retval;

    pthread_mutex_lock(&dataLock);
    retval = robot.max.range[pos-1];
    pthread_mutex_unlock(&dataLock);
    return retval;
}

int   SvrData::getMaxRange(float* data, size_t size)
{
    if (data == NULL)
        return -EINVAL;
    if (size != sizeof(float)* robot.joints)
        return -EINVAL;

    pthread_mutex_lock(&dataLock);
    memcpy(data,robot.max.range,size);
    pthread_mutex_unlock(&dataLock);
    return 0;
}
Vec<float, LBR_MNJ> SvrData::getMaxRange()
{
    Vec<float, LBR_MNJ> retval ;
    pthread_mutex_lock(&dataLock);
    retval = robot.max.range;
    pthread_mutex_unlock(&dataLock);
    return retval;
}

int SvrData::setRobotVelocity(float newVelocity)
{
    unsigned int retval = 0;

    pthread_mutex_lock(&dataLock);
    robot.currentVelocity = newVelocity;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
float SvrData::getRobotVelocity()
{
    float retval = 0;

    pthread_mutex_lock(&dataLock);
    retval = robot.currentVelocity;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
int SvrData::setRobotAcceleration(float newAcceleration)
{
    unsigned int retval = 0;

    pthread_mutex_lock(&dataLock);
    robot.currentAcceleration = newAcceleration;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
float SvrData::getRobotAcceleration()
{
    float retval = 0;

    pthread_mutex_lock(&dataLock);
    retval = robot.currentAcceleration;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
unsigned int SvrData::getJoints()
{
    unsigned int retval = 0;

    pthread_mutex_lock(&dataLock);
    retval = robot.joints;
    pthread_mutex_unlock(&dataLock);
    return retval;
}