#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)
{
    velocity = 20;
    accelaration = 10;
    robot.joints = LBR_MNJ;
    
    robot.max.velocity = new float[robot.joints];
    robot.max.accelaration = new float[robot.joints];
    robot.max.torque = new float[robot.joints];
    robot.max.range = new float[robot.joints];

    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};


    for (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()
{
    if (robot.max.velocity != 0)
        delete[] robot.max.velocity;

    if (robot.max.accelaration != 0)
        delete[] robot.max.accelaration;

    if (robot.max.torque != 0)
        delete[] robot.max.torque;

    if (robot.max.velocity != 0)
        delete[] robot.max.range;

    robot.max.velocity = 0;
    robot.max.accelaration = 0;
    robot.max.torque = 0;
    robot.max.range = 0;
    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);
}

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);
}

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);
}

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);
}

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);
}

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);
}

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);
}

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);
}

float SvrData::getMaxAccelaration(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);
}
int   SvrData::getMaxAccelaration(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);
}

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);
}

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);
}

unsigned int SvrData::getJoints()
{
    unsigned int retval = 0;

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