#include "commands.h"
#include "robot.h"
#include "lwr4.h"
#include "SocketObject.h"
#include "Trajectroy.h"
#include "SvrHandling.h"
#include "SvrData.h"

#include <stdlib.h>
#include <string>
#include "mat.h"
#include "vec.h"

#include "StringTool.h"
#include <iostream>
#include <iterator>
#include <algorithm>

#include <boost/thread/thread.hpp>

void getPositionJoints(SocketObject& client, std::string& arg)
{
    // unused
    (void) arg;

    std::string out = "";
    std::stringstream ss (std::stringstream::in | std::stringstream::out);

    // get current joint positions from database
    Robot::VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();

    // assemble command feadback
    for (int i=0; i< JOINT_NUMBER; i++)
    {
        ss.str("");
        ss << (jointPos(i)*180/M_PI);
        out += ss.str() + " ";
    }

    // send msg to client
    client.Send(out);
}

void getPositionHomRowWise(SocketObject& client, std::string& arg)
{
    // unused
    (void) arg;

    std::string out = "";
    std::stringstream ss (std::stringstream::in | std::stringstream::out);

    // get current Cartesian positions from database 
    MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();

    // assemble command feedback row wise
    for (int i=0; i< FRI_CART_FRM_DIM; i++)
    {
        int row = i % 4;
        int column = i / 4;

        ss.str("");
        ss << (cartPos(row,column));
        out += ss.str() + " ";
    }
    
    // send msg to client
    client.Send(out);
}

void getForceTorqueTcp(SocketObject& client, std::string& arg)
{
    // unused
    (void) arg;

    std::string out = "";
    std::stringstream ss (std::stringstream::in | std::stringstream::out);

    VecTorque torque = SvrData::getInstance()->getMessuredForceTorque();
    for (int i=0; i< FRI_CART_VEC; i++)
    {
        ss.str("");
        ss << torque(i);
        out += ss.str() + " ";
    }
    client.Send(out);
}

void movePTPJoints(SocketObject& client, std::string& arg)
{
    std::string buf;

    std::stringstream ss(arg);
    std::stringstream ss2f;

    // convert string to joint vector
    Robot::VecJoint newJointPos(0.0f);
    int i = 0;
    while (ss >> buf)
    {
        // to many joint values
        if (i>=JOINT_NUMBER)
        {
            client.Send(SVR_INVALID_ARGUMENT_COUNT);
            return;
        }
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> newJointPos(i);
        i++;
    }

    // to less joint values
    if (i!=JOINT_NUMBER)
    {
        client.Send(SVR_INVALID_ARGUMENT_COUNT);
        return ;
    }


    //check for joint range
    if (!SvrData::getInstance()->checkJointRange(newJointPos))
    {
        std::string out = "Error: Joints out of Range!";
        client.Send(SVR_OUT_OF_RANGE);
        return;
    }

    Trajectory<JOINT_NUMBER>* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);

    // add trajectory to queue for sender thread
    SvrData::getInstance()->pushTrajectory(newTrajectory);
    
    SvrData::getInstance()->setCommandedJointPos(newJointPos);

    //Wait to end of movement
    float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
    while (true)
    {
        if (SvrData::getInstance()->getTrajectoryDone() == true)
            break;
        boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
    }
    client.Send(SVR_TRUE_RSP);
}

void moveHomRowWiseStatus(SocketObject& client, std::string& arg)
{
    std::string buf;
    double temp[15];
    std::stringstream ss(arg);
    std::stringstream ss2f;
    int argc = 0;

    // convert position parameters from string to float
    while (ss >> buf)
    {
        // only need 15 parameters not more
        if (argc>15-1)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }

        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> temp[argc];
        argc++;
    }

    // check for exactly 15 parameters
    if (argc != 15)
    {
        client.Send(SVR_FALSE_RSP);
        return ;
    }

    // first part is homegenous position koordinates
    MatCarthesian newCartPos(0.0f, 1.0f);
    for (int i=0 ;i<3; i++)
    {
        for (int j=0;j<4;j++)
        {
            newCartPos(i,j)=temp[i*4+j];
        }
    }

    // extract elbow flip and hand parameter
    struct Robot::RobotConfig configurationParameter;
    configurationParameter.elbow = temp[12] == 1.0f;
    configurationParameter.flip  = temp[13] == 1.0f;
    configurationParameter.j1os  = temp[14] == 1.0f;

    //Backward Calculation
    Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter);


    //Check for Joint Range
    if (!SvrData::getInstance()->checkJointRange(newJointPos))
    {
        client.Send(SVR_OUT_OF_RANGE);
        return;
    }

    // calculate trajectory
    Robot::VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
    float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
    Robot::VecJoint maxJointVelocity     =  SvrData::getInstance()->getRobotVelocity();
    Robot::VecJoint maxJointAcceleration =  SvrData::getInstance()->getRobotAcceleration();
    std::cout << "max velo is " << maxJointVelocity ;

    class Trajectory<JOINT_NUMBER>* newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);

    // set new commanded position for next trajectory
    SvrData::getInstance()->setCommandedJointPos(newJointPos);

    // add new trajectory to position
    SvrData::getInstance()->pushTrajectory(newTrajectory);

    //Wait to end of movement
    while (true)
    {
        if (SvrData::getInstance()->getTrajectoryDone() == true)
            break;
        boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
    }

    // send a positive client feedback
    client.Send(SVR_TRUE_RSP);
}

void setSpeed(SocketObject& client, std::string& arg)
{
    std::string buf;
    float newVelocityPercentage;
    int argc = 0;
    std::stringstream ss(arg);
    std::stringstream ss2f;

    // no speed argument 
    if (arg == "")
    {
        client.Send(SVR_FALSE_RSP);
        return;
    }

    // convert speed argument to float
    while (ss >> buf)
    {
        // only need one speed argument
        if (argc>0)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> newVelocityPercentage;
        argc++;
    }
    
    // check for a valid range for the new Velocity 
    if ((newVelocityPercentage <= 0.0f)||(newVelocityPercentage >100.0f))
    {
        client.Send(SVR_FALSE_RSP);
        return;
    }

    // save new velocity into the database
    SvrData::getInstance()->setRobotVelocityPercentage(newVelocityPercentage);

    // signal a positive feedback to the client side
    client.Send(SVR_TRUE_RSP);
}

void setAccel(SocketObject& client, std::string& arg)
{
    std::string buf;
    float newAccelerationPercentage;
    std::stringstream ss(arg);
    std::stringstream ss2f;
    int argc = 0;

    // need at least one argument
    if (arg == "")
    {
        client.Send(SVR_FALSE_RSP);
        return;
    }
    
    // convert acceleration string into float
    while (ss >> buf)
    {
        // only one argument is necessary
        if (argc>0)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> newAccelerationPercentage;
        argc++;
    }

    // check for a valid rang for the robot acceleration
    if ((newAccelerationPercentage < 1.0f)||(newAccelerationPercentage >100.0f))
    {
        client.Send(SVR_FALSE_RSP);
            return;
    }

    // save new acceleration
    SvrData::getInstance()->setRobotAccelerationPercentage(newAccelerationPercentage);

    // signal a positive client feedback
    client.Send(SVR_TRUE_RSP);
}

void startPotFieldMode(SocketObject& client, std::string& arg)
{
    // unused
    (void) arg;
#if 0
    __MSRSTARTPOTFIELD = false;
    __MSRSTOPPOTFIELD = false;
    //Set current Joint Vals
    for (int i=0; i<LBR_MNJ; i++)
    {
        MSRCMDJNTPOS[i] = MSRMSRJNTPOS[i]*180/M_PI;
    }
    __POTFIELDMODE = true;
    while (1)
    {
        if (!__MSRSTARTPOTFIELD)
        {
            break;
        }
    }
#endif
    client.Send(SVR_TRUE_RSP);
}

void stopPotFieldMode(SocketObject& client, std::string& arg)
{
    // unused
    (void) arg;
#if 0
    __POTFIELDMODE = false;

    while (1)
    {
        if (!__MSRSTOPPOTFIELD)
        {
            break;
        }
    }
#endif
    client.Send(SVR_TRUE_RSP);
}

void setPos(SocketObject& client, std::string& arg)
{
    std::string buf;
    double temp[15];
    std::stringstream ss(arg);
    std::stringstream ss2f;
    int argc = 0;

    // convert position arguments from string to float
    while (ss >> buf)
    {
        // only 15 parameters are necessary
        if (argc>=15)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }

        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> temp[argc];
        argc++;
    }

    // check for exactly 15 parameters
    if (argc!=15)
    {
        client.Send(SVR_FALSE_RSP);
        return ;
    }


    // the position is saved in the first parameter part
    MatCarthesian newCartPos(0.0f,1.0f);
    for (int i=0 ;i<3; i++)
    {
        for (int j=0;j<4;j++)
        {
            newCartPos(i,j)=temp[i*4+j];
        }
    }

    // extract elbow flip and hand parameter
    struct Robot::RobotConfig configurationParameter;
    configurationParameter.elbow = temp[12] == 1.0f;
    configurationParameter.flip  = temp[13] == 1.0f;
    configurationParameter.j1os  = temp[14] == 1.0f;

    //Backward Calculation
    Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter);

    //Check for a  valid joint range
    if (!SvrData::getInstance()->checkJointRange(newJointPos))
    {
        client.Send(SVR_OUT_OF_RANGE);
        return;
    }

    // calculate new trajectory for next position
    
    // set new commanded Position for next Trajectory
    SvrData::getInstance()->setCommandedJointPos(newJointPos);

    //Wait to end of movement
    
    // send positive client feedback
    client.Send(SVR_TRUE_RSP);
}

void setJoints(SocketObject& client, std::string& arg)
{
    std::string buf;
    Robot::VecJoint newJointPos(0.0f);
    std::stringstream ss(arg);
    std::stringstream ss2f;
    int argc = 0;

    // convert to Joint vector
    while (ss >> buf)
    {
        // to many input arguments or Joints
        if (argc>=JOINT_NUMBER)
        {
            client.Send(SVR_INVALID_ARGUMENT_COUNT);
            return;
        }

        // convert string to float
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> newJointPos(argc);
        argc++;
    }

    // to less joint values
    if (argc != JOINT_NUMBER)
    {
        client.Send(SVR_INVALID_ARGUMENT_COUNT);
        return;
    }

    //Check for Joint Range
    if (!SvrData::getInstance()->checkJointRange(newJointPos))
    {
        client.Send(SVR_OUT_OF_RANGE);
        return;
    }

    // calculate trajectory from last commanded joint position
    Robot::VecJoint currentComandedJoint = SvrData::getInstance()->getCommandedJointPos();
    
    // set new trajectory
    
    // Set new target Position and also calculate Cartesian position
    SvrData::getInstance()->setCommandedJointPos(newJointPos);
    // TODO calculate and calculate position in Cartesian coordinates

    // wait till position is reached

    // return positive response to client
    client.Send(SVR_TRUE_RSP);
}

void moveCartesian(SocketObject& client, std::string& arg)
{
#if 0
    std::string buf;
    float temp[15];
    std::stringstream ss(arg);
    std::stringstream ss2f;
    int argc = 0;

    // convert Cartesian coordinates from string into float
    while (ss >> buf)
    {
        // only need maximum 16 coordinates for a homogeneous matrix
        if (argc>=15)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> temp[argc];
        argc++;
    }

    // check for exactly 15 coordinates
    if (argc != 15)
    {
        client.Send(SVR_FALSE_RSP);
        return ;
    }
    
    float configurationParameter[3];
    for (int i=0;i<3;i++)
    {
        configurationParameter[i] = temp[12+i];
    }

    //Calculating end-effector position for target "TRobot"
    Mat4f Tsurface;
    Tsurface = vecToMat(temp);
    Mat4f TRobot;
    TRobot = Tsurface;
    
    //Calculating end-effector position for approach position "ApRobot"
    Mat4f ApRobot;
    Mat4f ApSurface;
    Mat4f TransZ; 
    // 100mm approach position
    TransZ = getTranslation(0,0,-0.1);
    ApSurface = Tsurface * TransZ;
    ApRobot = ApSurface ;
    float* tempVec = NULL;
    tempVec = matToVec(TRobot);
    //tempVec = matToVec(ApRobot);
    
#endif 
    // send a positive client feedback
    client.Send(SVR_TRUE_RSP);
}

void quit(SocketObject& client , std::string& arg)
{
    //unused 
    (void) arg;

    client.Send(SVR_TRUE_RSP);
    client.Disconnect();
}

void setTrajectoryType(SocketObject& client, std::string& arg)
{
    std::string buf;
    enum TrajectoryType newType = TrajectoryDefault;

    newType = toEnum(arg);
    SvrData::getInstance()->setTrajectoryType(newType);
    std::cout << "new trajectory type : "<<  toString(newType) << "\n";
    
    client.Send(SVR_TRUE_RSP);
}
void getTrajectoryType(SocketObject& client, std::string& arg)
{
    enum TrajectoryType currType =  SvrData::getInstance()->getTrajectoryType();
    std::cout << "curr trajectory type : "<<  toString(currType) << "\n";

    client.Send("current trajectory: " + toString(currType));
}

void debug(SocketObject& client, std::string& arg)
{

    //check = true;
    //float temp[7];
    //while (1){
    //  friInst.setToKRLInt(15,1);
    //  friInst.doSendData();
    //  //client.Send(friInst.getFrmKRLInt(2));
    //  for ( int i= 0; i < LBR_MNJ; i++)
    //{
    //  temp[i]=friInst.getMsrMsrJntPosition()[i];
    //}
    //friInst.doPositionControl(temp);
    //}
}