#include "header.h"
#include <ctime>
#include "global.h"
#include "mat.h"
#include "StringTool.h"
#include <iostream>
#include <iterator>
#include <algorithm>

#include "SvrHandling.h"
#include "SvrData.h"
#include "Trajectroy.h"
#include "LinearTrajectory.h"
#include <boost/thread/thread.hpp>

void printUsage(SocketObject& client,  std::string& arg)
{
    (void) arg;
    for (unsigned int i = 0 ; i < commandCount; ++i)
    {
        if(commands[i].printHelp != NULL)
            commands[i].printHelp();
        else
            client.Send(commands[i].aberration+"\t-\t"+commands[i].longVersion);
    }
}

VecJoint kukaBackwardCalc(double M_[12], float arg[3])
{
    VecJoint Joints(0.0f);
    //DH-Parameter
    float alp[7] = {-M_PI/2.0f,     M_PI/2.0f, -M_PI/2.0f,     M_PI/2.0f, -M_PI/2.0f,     M_PI/2.0f, 0.0f};
    float d[7]   = {310.4f/1000.0f, 0.0f,      400.1f/1000.0f, 0.0f,      390.0f/1000.0f, 0.0f,      78.0f/1000.0f};
    //Parameter
    int ELBOW = arg[0];
    int FLIP = arg[1];
    float J1os = arg[2]/180.0*M_PI;

    //Calculating M06
    Mat4f R67;
    int i = 6;

    R67(0,0) = cos(0);
    R67(0,1) = -sin(0)*cos(alp[i]);
    R67(0,2) = sin(0)*sin(alp[i]);
    R67(0,3) = 0;

    R67(1,0) = sin(0);
    R67(1,1) = cos(0)*cos(alp[i]);
    R67(1,2) = -cos(0)*sin(alp[i]);
    R67(1,3) = 0;

    R67(2,0) = 0;
    R67(2,1) = sin(alp[i]);
    R67(2,2) = cos(alp[i]);
    R67(2,3) = d[i];

    R67(3,0) = 0;
    R67(3,1) = 0;
    R67(3,2) = 0;
    R67(3,3) = 1;

    Mat4f M;
    for (int i=0;i<3;i++)
    {
        for (int j=0;j<4;j++)
        {
            M(i,j) = (float)M_[i*4+j];
        }
    }
    M(3,3)=(float)1;
    Mat4f M06;
    Mat4f Inv;
    Inv = R67.inv();
    M06=M*Inv;

    //Joint 1 including given offset
    Joints(0) = atan2(M06(1,3),M06(0,3))+J1os;

    //Calculating M16
    Mat4f R01;
    i = 0;
    R01(0,0) = cos(Joints(i));
    R01(0,1) = -sin(Joints(i))*cos(alp[i]);
    R01(0,2) = sin(Joints(i))*sin(alp[i]);
    R01(0,3) = 0;

    R01(1,0) = sin(Joints(i));
    R01(1,1) = cos(Joints(i))*cos(alp[i]);
    R01(1,2) = -cos(Joints(i))*sin(alp[i]);
    R01(1,3) = 0;

    R01(2,0) = 0;
    R01(2,1) = sin(alp[i]);
    R01(2,2) = cos(alp[i]);
    R01(2,3) = d[i];

    R01(3,0) = 0;
    R01(3,1) = 0;
    R01(3,2) = 0;
    R01(3,3) = 1;

    Inv = R01.inv();
    Mat4f M16;
    M16 = Inv*M06;

    //Joint 2
    float m14 = M16(0,3);
    float m24 = M16(1,3);
    float m34 = M16(2,3);
    float d3 = d[2];
    float d5 = d[4];

    float arg1 = (0.5*(ELBOW*m24*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14
        -ELBOW*m24*m24*m24+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3
        +2*m14*m14*m14*m14*d5*d5+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14
        -2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14
        +2*m24*m24*d3*d3*m14*m14-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34
        +2*m14*m14*d5*d5*d3*d3+2*m24*m24*d5*d5*m14*m14))/(m24*m24+m14*m14)
        +m34*m34+d3*d3-1*d5*d5+m14*m14+m24*m24))/(d3*m14);

    float arg2 = ELBOW*(0.5*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14-ELBOW*m24*m24*m24
        +sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3+2*m14*m14*m14*m14*d5*d5
        +2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24
        *m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14+2*m24*m24*d3*d3*m14*m14
        -2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34+2*m14*m14*d5*d5*d3*d3
        +2*m24*m24*d5*d5*m14*m14)))/((m24*m24+m14*m14)*d3);

    Joints(1) = atan2(arg1,arg2);

    //Calculating M26
    Mat4f R12;
    i = 1;

    R12(0,0) = cos(Joints(i));
    R12(0,1) = -sin(Joints(i))*cos(alp[i]);
    R12(0,2) = sin(Joints(i))*sin(alp[i]);
    R12(0,3) = 0;

    R12(1,0) = sin(Joints(i));
    R12(1,1) = cos(Joints(i))*cos(alp[i]);
    R12(1,2) = -cos(Joints(i))*sin(alp[i]);
    R12(1,3) = 0;

    R12(2,0) = 0;
    R12(2,1) = sin(alp[i]);
    R12(2,2) = cos(alp[i]);
    R12(2,3) = d[i];

    R12(3,0) = 0;
    R12(3,1) = 0;
    R12(3,2) = 0;
    R12(3,3) = 1;

    Mat4f R02;
    R02 = R01*R12;
    Inv = R02.inv();
    Mat4f M26;
    M26 = Inv*M06;

    //Joint 3
    Joints(2) = atan2(ELBOW*M26(1,3),ELBOW*M26(0,3));

    //Calculating M36
    Mat4f R23;
    i = 2;

    R23(0,0) = cos(Joints(i));
    R23(0,1) = -sin(Joints(i))*cos(alp[i]);
    R23(0,2) = sin(Joints(i))*sin(alp[i]);
    R23(0,3) = 0;

    R23(1,0) = sin(Joints(i));
    R23(1,1) = cos(Joints(i))*cos(alp[i]);
    R23(1,2) = -cos(Joints(i))*sin(alp[i]);
    R23(1,3) = 0;

    R23(2,0) = 0;
    R23(2,1) = sin(alp[i]);
    R23(2,2) = cos(alp[i]);
    R23(2,3) = d[i];

    R23(3,0) = 0;
    R23(3,1) = 0;
    R23(3,2) = 0;
    R23(3,3) = 1;

    Mat4f R03;
    R03 = R01*R12*R23;
    Inv = R03.inv();
    Mat4f M36;
    M36 = Inv*M06;

    //Joint 4
    Joints(3) = atan2(M36(0,3),-M36(1,3));

    //Calculating M47
    Mat4f R34;
    i = 3;
    R34(0,0) = cos(Joints(i));
    R34(0,1) = -sin(Joints(i))*cos(alp[i]); 
    R34(0,2) = sin(Joints(i))*sin(alp[i]);
    R34(0,3) = 0;

    R34(1,0) = sin(Joints(i));
    R34(1,1) = cos(Joints(i))*cos(alp[i]);
    R34(1,2) = -cos(Joints(i))*sin(alp[i]);
    R34(1,3) = 0;

    R34(2,0) = 0;
    R34(2,1) = sin(alp[i]);
    R34(2,2) = cos(alp[i]);
    R34(2,3) = d[i];

    R34(3,0) = 0;
    R34(3,1) = 0;
    R34(3,2) = 0;
    R34(3,3) = 1;

    Mat4f R04;
    R04 = R01*R12*R23*R34;
    Inv = R04.inv();
    Mat4f M47;
    M47 = Inv*M;

    //Joint 5
    float th = 0.0001;
    if (abs(M47(1,2))<th && abs(M47(0,2))<th)
        Joints(4) = 0;
    else
        Joints(4) = atan2(FLIP*M47(1,2),FLIP*M47(0,2));

    //Joint 6
    Joints(5) = atan2(FLIP*sqrt(1-M47(2,2)*M47(2,2)),M47(2,2));

    //Joint 7
    if (abs(M47(2,1))<th && abs(M47(2,0))<th)
        Joints(6) = 0;
    else
        Joints(6) = atan2(FLIP*M47(2,1),FLIP*-M47(2,0));

    for (int i = 0; i<7; i++)
    {
        Joints(i) = Joints(i)*180.0/M_PI;
    }

    //Kuka Joints
    //Joints[3] = -Joints[3];
    Joints(1) = -Joints(1);
    Joints(5) = -Joints(5);

    return Joints;
}

Mat4f vecToMat(float vec[12])
{
    Mat4f result;
    for (int i=0; i<3; i++) // ZEILE
    {
        for (int j=0; j<4; j++) // SPALTE
        {
            result(i,j) = vec[i*4+j];
        }
    }
    result(3,3)= 1.0f;
    return result;
}

float* matToVec(Mat4f mat)
{
    float* vec = new float[12];
    for (int j=0;j<3;j++)
    {
        for (int k=0;k<4;k++)
        {
            vec[j*4+k] = mat(j,k);
        }
    }
    return vec;
}

Mat4f getTranslation(double tx, double ty, double tz)
{
    Mat4f transl;
    transl(0,0) = 1;
    transl(1,1) = 1;
    transl(2,2) = 1;

    transl(0,3) = tx;
    transl(1,3) = ty;
    transl(2,3) = tz;
    transl(3,3) = 1;
    return transl;
}

SvrHandling::SvrHandling()
{
    // add each command which is possible
    commands = new ClientCommand[20];

    int i = 0;
    commands[i].aberration     = "GPJ";
    commands[i].longVersion    = "GetPositionJoints";
    commands[i].processCommand = &getPositionJoints;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "GPHRW";
    commands[i].longVersion    = "GetPositionHomRowWise";
    commands[i].processCommand = &getPositionHomRowWise;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "GFT";
    commands[i].longVersion    = "GetForceTorqueTcp";
    commands[i].processCommand = &getForceTorqueTcp;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "MPTPJ";
    commands[i].longVersion    = "MovePTPJoints";
    commands[i].processCommand = &movePTPJoints;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "MHRWS";
    commands[i].longVersion    = "MoveHomRowWiseStatus";
    commands[i].processCommand = &moveHomRowWiseStatus;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SS";
    commands[i].longVersion    = "SetSpeed";
    commands[i].processCommand = &setSpeed;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SA";
    commands[i].longVersion    = "SetAccel";
    commands[i].processCommand = &setAccel;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "STPF";
    commands[i].longVersion    = "StartPotFieldMode";
    commands[i].processCommand = &startPotFieldMode;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SPPF";
    commands[i].longVersion    = "StopPotFieldMode";
    commands[i].processCommand = &stopPotFieldMode;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SP";
    commands[i].longVersion    = "SetPos";
    commands[i].processCommand = &setPos;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "SJ";
    commands[i].longVersion    = "SetJoints";
    commands[i].processCommand = &setJoints;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "Q";
    commands[i].longVersion    = "Quit";
    commands[i].processCommand = &quit;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "?";
    commands[i].longVersion    = "Help";
    commands[i].processCommand = &printUsage;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "ISKUKA";
    commands[i].longVersion    = "IsKukaLWR";
    commands[i].processCommand = NULL;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "MC";
    commands[i].longVersion    = "MoveCartesian";
    commands[i].processCommand = &moveCartesian;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "GT";
    commands[i].longVersion    = "GetTrajectoryType";
    commands[i].processCommand = &getTrajectoryType;
    commands[i].printHelp      = NULL;
    i+=1;
    commands[i].aberration     = "ST";
    commands[i].longVersion    = "SetTrajectoryType";
    commands[i].processCommand = &setTrajectoryType;
    commands[i].printHelp      = NULL;

    commandCount = i;
}

SvrHandling::~SvrHandling()
{
}

void SvrHandling::run()
{
    this->run(SVR_DEFAULT_PORT);
}

void SvrHandling::run(int port)
{
    cout << timestamp() + "Starting " << SVR_NAME << " on port "<< port <<"\n";
    while (c_state !=done)
    {
        SocketObject *socket = new SocketObject;
        if (socket->Bind(port) <0)
        {
            exit (1);
        }
        socket->Listen();
        while(1)
        {
            SocketObject *client = new SocketObject;
            if (socket->Accept(*client))
            {
                cout << timestamp() + "Client accepted!\n";
                if (handshakeAccepted(*client))
                {
                    clientCommandLoop(*client);
                }
                client->Disconnect();
                cout << timestamp() + "Client disconnected.\n";
            }
            else
            {
                cout << timestamp() + "Client bin error.\n";
            }
            delete client;
        }
        delete socket;
    }
}

bool SvrHandling::handshakeAccepted(SocketObject& client)
{
    this->c_state = handshake;
    string message = SVR_HELLO_MSG;
    client.Send(message);
    client.Recv(message);
    if (message.find(SVR_HANDSHAKE,0) != string::npos)
    {
        c_state = accepted;
        message = SVR_ACCEPTED;
    }
    else
    {
        c_state = waiting;
        message = SVR_FAILED;
        cout << timestamp() + "Handshake failed. " << "Invalid recv msg \'" << message <<"\'";
    }
    client.Send(message);
    return(c_state == accepted);
}

void SvrHandling::clientCommandLoop(SocketObject& client)
{
    string message, cmd, arg;
    while (c_state == accepted) 
    {
        if (client.Recv(message) > 0) 
        {
            cout<<message;
            StringTool::String2CmdArg((const string) message, cmd, arg);

            // ignore blank lines
            if (cmd == "") 
                continue;

            // check command table for equivalence
            bool match = false;
            for (unsigned int i = 0 ; i < commandCount ; ++ i)
            {
                if (cmd == commands[i].longVersion || cmd == commands[i].aberration)
                {
                    if (commands[i].processCommand != NULL)
                    {
                        commands[i].processCommand(client,arg);
                    }else
                    {
                        client.Send(SVR_UNIMPLEMENTED_COMMAND);
                    }
                    match = true;
                    match = true;
                    break;
                }
            }
            // command not found
            if (match == false)
            {
                client.Send(SVR_UNKNOWN_COMMAND);
                printUsage(client, arg);
            }

        }else 
        {
            cout << timestamp() + "Connection to client lost..\n";
            c_state = waiting;
        }
    }
}

string SvrHandling::timestamp()
{
    time_t rawtime;
    struct tm * timeinfo;
    char buffer [80];

    time ( &rawtime );
    timeinfo = localtime ( &rawtime );

    strftime (buffer,80,"%Y-%m-%d-%H-%M-%S",timeinfo);
    string temp = ": ";
    string output = buffer + temp;
    return output;
}

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

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

    // get current joint positions from database
    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;

    string out = "";
    stringstream ss (stringstream::in | 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;

    string out = "";
    stringstream ss (stringstream::in | 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, string& arg)
{
    string buf;

    stringstream ss(arg);
    stringstream ss2f;

    // convert string to joint vector
    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))
    {
        string out = "Error: Joints out of Range!";
        client.Send(SVR_OUT_OF_RANGE);
        return;
    }
    //set new target for next trajectory
    VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
    class Trajectory<JOINT_NUMBER>* newTrajectory;
    
    float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
    VecJoint maxJointVelocity     =  SvrData::getInstance()->getMaxVelocity();
    VecJoint maxJointAcceleration =  SvrData::getInstance()->getMaxAcceleration();

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

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

    //Wait to end of movement
    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, string& arg)
{
    string buf;
    double temp[15];
    stringstream ss(arg);
    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
    float configurationParameter[3];
    for (int i=0;i<3;i++)
    {
        configurationParameter[i] = temp[12+i];
    }

    //Backward Calculation
    VecJoint newJointPos = kukaBackwardCalc(temp, configurationParameter);


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

    // calculate trajectory
    VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
    float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
    VecJoint maxJointVelocity     =  SvrData::getInstance()->getMaxVelocity();
    VecJoint maxJointAcceleration =  SvrData::getInstance()->getMaxAcceleration();

    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, string& arg)
{
    string buf;
    float newVelocityPercentage;
    int argc = 0;
    stringstream ss(arg);
    stringstream ss2f;
    vector<string> tokens;

    // 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, string& arg)
{
    string buf;
    float newAccelerationPercentage;
    stringstream ss(arg);
    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, 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, string& arg)
{
    // unused
    (void) arg;
#if 0
    __POTFIELDMODE = false;

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

void setPos(SocketObject& client, string& arg)
{
    string buf;
    double temp[15];
    stringstream ss(arg);
    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
    float configurationParameter[3];
    for (int i=0;i<3;i++)
    {
        configurationParameter[i] = temp[12+i];
    }

    //Backward Calculation of the position
    VecJoint CalcJoints = kukaBackwardCalc(temp, configurationParameter);

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

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

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

void setJoints(SocketObject& client, string& arg)
{
    string buf;
    VecJoint newJointPos(0.0f);
    stringstream ss(arg);
    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
    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, string& arg)
{
    string buf;
    float temp[15];
    stringstream ss(arg);
    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);
    
    // 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)
{
    string buf;
    enum TrajectoryType newType = TrajectoryDefault;

    newType = toEnum(arg);
    SvrData::getInstance()->setTrajectoryType(newType);
    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();
    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);
    //}
}