#include <pthread.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include "Trajectroy.h"
#include "SvrData.h"
#include "friComm.h"
#include "friremote.h"
#include <boost/thread/thread.hpp>

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;

    for (unsigned int i = 0; i < JOINT_NUMBER; ++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];
    }

    trajectory.sampleTimeMs = 5.0f;
    trajectory.cancel = false;

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

    this->friInst = new friRemote;
}

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

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

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

friRemote* SvrData::getFriRemote()
{
    friRemote* retval = NULL;

    pthread_mutex_lock(&dataLock);
    retval = this->friInst;
    pthread_mutex_unlock(&dataLock);
    return retval;
}

void SvrData::updateMessurementData(
        VecJoint newJointPos,
        MatCarthesian newCartPos,
        VecTorque newForceAndTorque
        )
{
    pthread_mutex_lock(&dataLock);
    this->messured.jointPos = newJointPos;
    this->messured.cartPos  = newCartPos;
    this->messured.forceAndTorque = newForceAndTorque;
    pthread_mutex_unlock(&dataLock);
}

void SvrData::updateMessurement()
{
    VecJoint newJointPos(0.0f);
    MatCarthesian newCartPos(0.0f,1.0f);
    VecTorque newForceAndTorque(0.0f);

    /// do the update using fri
    /// update current joint positions
    VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition());
    VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset());
    newJointPos = newJointPosComanded + newJointPosOffset;

    /// update current cart position
    newCartPos = friInst->getMsrCartPosition();
    
    /// update current force and torque
    newForceAndTorque = friInst->getFTTcp();

    updateMessurementData(newJointPos,newCartPos,newForceAndTorque);
}


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

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

VecJoint SvrData::getMessuredJointPos()
{
    VecJoint buf;
    pthread_mutex_lock(&dataLock);
    buf = messured.jointPos;
    pthread_mutex_unlock(&dataLock);
    return buf;
}

MatCarthesian SvrData::getMessuredCartPos()
{
    MatCarthesian buf;
    pthread_mutex_lock(&dataLock);
    buf = messured.cartPos;
    pthread_mutex_unlock(&dataLock);
    return buf;
}
VecTorque SvrData::getMessuredForceTorque()
{
    VecTorque buf;
    pthread_mutex_lock(&dataLock);
    buf = messured.forceAndTorque;
    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;
}

VecJoint SvrData::getCommandedJointPos()
{
    VecJoint buff; 
    pthread_mutex_lock(&dataLock);
    buff = commanded.jointPos;
    pthread_mutex_unlock(&dataLock);
    return buff;
}
void SvrData::setCommandedJointPos(VecJoint newJointPos)
{
    pthread_mutex_lock(&dataLock);
    commanded.jointPos = newJointPos;
    pthread_mutex_unlock(&dataLock);
}
MatCarthesian SvrData::getCommandedCartPos ()
{
    MatCarthesian buff; 
    pthread_mutex_lock(&dataLock);
    buff = commanded.cartPos;
    pthread_mutex_unlock(&dataLock);
    return buff;
}
void SvrData::setCommandedCartPos(MatCarthesian newCartPos)
{
    pthread_mutex_lock(&dataLock);
    commanded.cartPos = newCartPos;
    pthread_mutex_unlock(&dataLock);
}


VecJoint SvrData::getMaxTorque()
{
    VecJoint buff;
    pthread_mutex_lock(&dataLock);
    buff = robot.max.torque;
    pthread_mutex_unlock(&dataLock);
    return buff;
}

VecJoint SvrData::getMaxAcceleration()
{
    VecJoint buff;
    pthread_mutex_lock(&dataLock);
    buff = robot.max.accelaration;
    pthread_mutex_unlock(&dataLock);
    return buff;
}

VecJoint SvrData::getMaxVelocity()
{
    VecJoint buff;

    pthread_mutex_lock(&dataLock);
    buff = robot.max.velocity;
    pthread_mutex_unlock(&dataLock);
    return buff;
}
VecJoint SvrData::getMaxRange()
{
    VecJoint buff;

    pthread_mutex_lock(&dataLock);
    buff = robot.max.range;
    pthread_mutex_unlock(&dataLock);
    return buff;
}
bool SvrData::checkJointRange(VecJoint vectorToCheck)
{
    bool retval = false;

    pthread_mutex_lock(&dataLock);
    Vec<float,LBR_MNJ> maxJointRange;
    maxJointRange = robot.max.range;
    pthread_mutex_unlock(&dataLock);
    
    for (int i = 0; i<JOINT_NUMBER; i++)
    {
        if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
        {
            // join angle is too big
            return false;
        }
    }
    // no join angle is too big 
    return false;
}

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 = JOINT_NUMBER;
    pthread_mutex_unlock(&dataLock);
    return retval;
}

/// Trajectory function
float SvrData::getSampleTimeMs()
{
    float retval = 0;

    pthread_mutex_lock(&dataLock);
    retval = trajectory.sampleTimeMs;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
int   SvrData::setSampleTimeMs(float newSampleTimeMs)
{
    int retval = 0;
    
    // sample time can not be negative or 0
    if (newSampleTimeMs <= 0.0f)
        return -1;

    pthread_mutex_lock(&dataLock);
    trajectory.sampleTimeMs = newSampleTimeMs;
    pthread_mutex_unlock(&dataLock);
    return retval;
}

class Trajectory<JOINT_NUMBER>*  SvrData::popTrajectory()
{
    class Trajectory<JOINT_NUMBER>* retval = NULL;
    pthread_mutex_lock(&dataLock);
    retval = trajectory.list.front();
    pthread_mutex_unlock(&dataLock);
    return retval;
}
int SvrData::pushTrajectory(class Trajectory<JOINT_NUMBER>* newTrajectory)
{
    if (newTrajectory == NULL)
        return -1;

    pthread_mutex_lock(&dataLock);
    trajectory.list.push(newTrajectory);
    pthread_mutex_unlock(&dataLock);
    return 0;
}
bool SvrData::getTrajectoryCancel()
{
    bool retval = false;
    pthread_mutex_lock(&dataLock);
    retval = trajectory.cancel;
    pthread_mutex_unlock(&dataLock);
    return retval;
}

bool SvrData::cancelCurrentTrajectory()
{
    pthread_mutex_lock(&dataLock);
    // set cancel state 
    trajectory.cancel = true;
    
    // clear trajectory list
    while(!trajectory.list.empty())
    {
        class Trajectory<JOINT_NUMBER>* currentTrajectory = trajectory.list.front();
        if(currentTrajectory != NULL)
            delete currentTrajectory;
        trajectory.list.pop();
    }

    pthread_mutex_unlock(&dataLock);

    // wait for stop signaled by calling cancel done
    bool run = true;
    while (run)
    {
        pthread_mutex_lock(&dataLock);
        if (trajectory.cancel == false)
        {
            run = false;
        }
        pthread_mutex_unlock(&dataLock);
        boost::this_thread::sleep( boost::posix_time::milliseconds(trajectory.sampleTimeMs) );
    }
    return true;
}
void SvrData::trajectoryCancelDone()
{
    pthread_mutex_lock(&dataLock);
    trajectory.cancel = false;
    pthread_mutex_unlock(&dataLock);
}