#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.currentVelocityPercentage = 20;
    robot.currentAccelerationPercentage = 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
    friInst->getState();
	FRI_STATE state = friInst->getState();
    VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition());
    VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset());
    newJointPos = newJointPosComanded + newJointPosOffset;
    newJointPos = newJointPos * ( 180.0f / M_PI);

    /// update current cart position
    newCartPos = friInst->getMsrCartPosition();
    
    /// update current force and torque
    newForceAndTorque = friInst->getFTTcp();
    std::cout << newJointPos << " current newJointPos \n" << state << "\n";

    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 retval(0.0f);
    pthread_mutex_lock(&dataLock);
    retval = robot.max.accelaration;
    pthread_mutex_unlock(&dataLock);
    return retval;
}

VecJoint SvrData::getMaxVelocity()
{
    VecJoint retval(0.0f);
    pthread_mutex_lock(&dataLock);
    retval = robot.max.velocity;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
VecJoint SvrData::getMaxRange()
{
    VecJoint retval(0.0f);
    pthread_mutex_lock(&dataLock);
    retval = robot.max.range;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
bool SvrData::checkJointRange(VecJoint vectorToCheck)
{
    // save temporal the max range for short time lock
    pthread_mutex_lock(&dataLock);
    Vec<float,LBR_MNJ> maxJointRange;
    maxJointRange = robot.max.range;
    pthread_mutex_unlock(&dataLock);
    
    // check if the value is in plus or minus range 
    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 true;
}

int SvrData::setRobotVelocityPercentage(float newVelocityPercentage)
{
    unsigned int retval = 0;
    // percentage is to low
    if (newVelocityPercentage <= 0.0f)
        return -1;
    // percentage is to high
    if (newVelocityPercentage > 100.0f)
        return -2;

    pthread_mutex_lock(&dataLock);
    robot.currentVelocityPercentage = newVelocityPercentage;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
VecJoint SvrData::getRobotVelocity()
{
    float percent = 0.0f;
    VecJoint retval(0.0f);

    pthread_mutex_lock(&dataLock);
    percent = robot.currentVelocityPercentage / 100.0f;
    std::cout << "percent " << percent << " velo " << retval;
    std::cout << robot.max.velocity << "max velo \n";
    retval = robot.max.velocity * percent;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
float SvrData::getRobotVelocityPercentage()
{
    float retval = 0;

    pthread_mutex_lock(&dataLock);
    retval = robot.currentVelocityPercentage;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage)
{
    int retval = 0;
    // percentage is to low
    if (newAccelerationPercentage <= 0.0f)
        return -1;
    // percentage is to high
    if (newAccelerationPercentage > 100.0f)
        return -2;

    pthread_mutex_lock(&dataLock);
    robot.currentAccelerationPercentage = newAccelerationPercentage;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
VecJoint SvrData::getRobotAcceleration()
{
    float percent = 0;
    VecJoint retval(0.0f);

    pthread_mutex_lock(&dataLock);
    percent = robot.currentAccelerationPercentage / 100.0f;
    retval  = robot.max.accelaration * percent;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
float SvrData::getRobotAccelerationPercentage()
{
    float retval = 0;

    pthread_mutex_lock(&dataLock);
    retval = robot.currentAccelerationPercentage;
    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);
    if (!trajectory.list.empty() )
    {
        retval = trajectory.list.front();
        trajectory.list.pop();
    }
    else
    {
        // no trajectroy left
        trajectory.done = true;
    }
    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);
    trajectory.done = false;
    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);
}

bool SvrData::getTrajectoryDone()
{
    bool retval = true;
    pthread_mutex_lock(&dataLock);
    retval = trajectory.done;
    pthread_mutex_unlock(&dataLock);
    return retval;
}

enum TrajectoryType SvrData::getTrajectoryType()
{
    enum TrajectoryType retval = TrajectoryDefault;
    pthread_mutex_lock(&dataLock);
    retval = trajectory.type;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
void SvrData::setTrajectoryType(enum TrajectoryType newType)
{
    pthread_mutex_lock(&dataLock);
    trajectory.type = newType;
    pthread_mutex_unlock(&dataLock);
}

class Trajectory<JOINT_NUMBER>* SvrData::createTrajectory(VecJoint newJointPos)
{
    //set new target for next trajectory
    VecJoint prevJointPos = getCommandedJointPos();
    class Trajectory<JOINT_NUMBER>* newTrajectory;
    
    float sampleTimeMs = getSampleTimeMs();
    VecJoint maxJointVelocity     =  getRobotVelocity();
    VecJoint maxJointAcceleration =  getRobotAcceleration();
    enum TrajectoryType type      =  getTrajectoryType();

    //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
    newTrajectory = Trajectory<JOINT_NUMBER>::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); 

    return newTrajectory;
}
class Trajectory<JOINT_NUMBER>* SvrData::createTrajectory(MatCarthesian newJointPos)
{

}
VecJoint SvrData::getMeasuredJointPos()
{
    VecJoint retval(0.0f);
    pthread_mutex_lock(&dataLock);
    retval = messured.jointPos;
    pthread_mutex_unlock(&dataLock);
    return retval;
}
MatCarthesian getMeasuredCartPos();