#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 <boost/thread/thread.hpp>

void printUsage(SocketObject& client)
{
    client.Send(SVR_HELP_MSG);

}

void mattoabc(float M[3][4], float &a, float &b, float &c)
{
    float norm;
    float sa;
    float ca;

    norm = sqrt((M[0][0]*M[0][0])+(M[1][0]*M[1][0]));

    if (norm>1e-5)
    {
        sa = M[1][0]/norm;
        ca = M[0][0]/norm;
        a  = atan2(sa,ca);
    } else {
        sa = 0;
        ca = 1;
        a = 0;
    }
    b = atan2(-M[2][0],ca*M[0][0]+sa*M[1][0]);
    c = atan2(sa*M[0][2]-ca*M[1][2],-sa*M[0][1]+ca*M[1][1]);
}

void debugMat(Mat4f M)
{
    cout << M << "\n\r" << endl;
}

double* kukaBackwardCalc(double M_[12], float arg[3])
{
    double* Joints = new double[7];
    //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()
{
}

SvrHandling::~SvrHandling()
{
}

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

void SvrHandling::run(int port)
{
    cout << timestamp() + "Starting " << SVR_NAME << "\n";
    port = SVR_DEFAULT_PORT;
    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))
                {
                    handle(*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::handle(SocketObject& client)
{
    string message, cmd, arg;
    while (c_state == accepted) 
    {
        if (client.Recv(message) > 0) 
        {
            cout<<message;
            StringTool::String2CmdArg((const string) message, cmd, arg);

            //SvrData::getInstance().lock();

            if (cmd == CMD_GetPositionJoints)
            {
                GetPositionJoints(client);
            } else if (cmd == CMD_GetPositionHomRowWise)
            {
                GetPositionHomRowWise(client);
            } else if (cmd == CMD_GetForceTorqueTcp)
            {
                GetForceTorqueTcp(client);
            } else if (cmd == CMD_MovePTPJoints)
            {
                MovePTPJoints(client, arg);
            } else if (cmd == CMD_MoveHomRowWiseStatus)
            {
                MoveHomRowWiseStatus(client, arg);
                // TODO: MoveMinChangeHomRowWiseStatus
            } else if (cmd == CMD_SetSpeed)
            {
                SetSpeed(client, arg);
            } else if (cmd == CMD_SetAccel)
            {
                SetAccel(client, arg);
            }else if (cmd == CMD_StartPotFieldMode)
            {
                StartPotFieldMode(client, arg);
            }else if (cmd == CMD_StopPotFieldMode)
            {
                StopPotFieldMode(client, arg);
            }else if (cmd == CMD_SetPos)
            {
                SetPos(client, arg);
            }else if (cmd == CMD_SetJoints)
            {
                SetJoints(client, arg);
            }else if (cmd == CMD_MoveCartesian)
            {
                MoveCartesian(client, arg);
            } else if (cmd == "debug")
            {
                debug(client);
            } else if (cmd == "") 
            {
                continue;
            } else if (cmd == CMD_HELP)
            {
                printUsage(client);
            } else if (cmd == CMD_QUIT || cmd == CMD_QUIT1 || cmd == CMD_QUIT2)
            {
                quit(client);
            } else if (cmd == CMD_ISKUKA)
            {
                client.Send(SVR_TRUE_RSP);
            } else 
            {
                message = "Error: Unknown command!";
                client.Send(message);
            }
            //SvrData::getInstance().unlock();
        }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 SvrHandling::GetPositionJoints(SocketObject& client)
{
    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 SvrHandling::GetPositionHomRowWise(SocketObject& client)
{
    string out = "";
    stringstream ss (stringstream::in | stringstream::out);

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

    // assemble command feadback 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 SvrHandling::GetForceTorqueTcp(SocketObject& client)
{
    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 SvrHandling::MovePTPJoints(SocketObject& client, string& args)
{
    //__SVR_CURRENT_JOB = true;
    string buf;

    stringstream ss(args);
    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 = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);

    __MSRCMDJNTPOS = true;

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

void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args)
{
    __SVR_CURRENT_JOB = true;
    string buf;
    double temp[15];
    stringstream ss(args);
    stringstream ss2f;
    vector<string> tokens;
    int i = 0;
    while (ss >> buf)
    {
        if (i>15-1)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        tokens.push_back(buf);
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> temp[i];
        i++;
    }
    if (i<15-1)
    {
        client.Send(SVR_FALSE_RSP);
        return ;
    }
    for (int i=0 ;i<3; i++)
    {
        for (int j=0;j<4;j++)
        {
            MSRCMDPOSE[i][j]=temp[i*4+j];
        }
    }
    float arg[3];
    for (int i=0;i<3;i++)
    {
        arg[i] = temp[12+i];
    }

    //Backward Calculation

    double* CalcJoints=NULL;
    CalcJoints = kukaBackwardCalc(temp, arg);

    //Check for Joint Range
    if (!checkJntRange(CalcJoints))
    {
        string out = "Error: Joints out of Range!";
        client.Send(out);
        return;
    }

    // Jmove
    for (int i=0 ;i<LBR_MNJ; i++)
    {
        MSRCMDJNTPOS[i]=CalcJoints[i];
    }
    __MSRCMDJNTPOS = true;

    //Wait to end of movement
    while (1)
    {
        if (!__MSRCMDJNTPOS)
        {
            break;
        }
    }
    client.Send(SVR_TRUE_RSP);

}

void SvrHandling::SetSpeed(SocketObject& client, string& args)
{
    __SETVELOCITY = true;
    __SVR_CURRENT_JOB = true;
    //SvrData::getInstance()->setState(SVR_STATE_SET_VELOCITY);
    string buf;
    float newVelocity;
    stringstream ss(args);
    stringstream ss2f;
    vector<string> tokens;
    int i = 0;
    if (args == "")
    {
        client.Send(SVR_FALSE_RSP);
        return;
    }
    while (ss >> buf)
    {
        if (i>0)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        tokens.push_back(buf);
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> newVelocity;
        i++;
    }
    if ((newVelocity < 1.0f)||(newVelocity >1000.0))
    {
        client.Send(SVR_FALSE_RSP);
        return;
    }
    SvrData::getInstance()->setRobotVelocity(newVelocity);
    client.Send(SVR_TRUE_RSP);
    //SvrData::getInstance()->setState(SVR_STATE_RUNNING);
    __SVR_CURRENT_JOB = false;
}

void SvrHandling::SetAccel(SocketObject& client, string& args)
{
    __SETACCEL = true;
    __SVR_CURRENT_JOB = true;
    string buf;
    float newAcceleration;
    stringstream ss(args);
    stringstream ss2f;
    vector<string> tokens;
    int i = 0;

    if (args == "")
    {
        client.Send(SVR_FALSE_RSP);
        return;
    }
    while (ss >> buf)
    {
        if (i>0)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        tokens.push_back(buf);
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> newAcceleration;
        i++;
    }
    if ((newAcceleration < 1.0f)||(newAcceleration >100.0f))
    {
        client.Send(SVR_FALSE_RSP);
            return;
    }
    SvrData::getInstance()->setRobotAcceleration(newAcceleration);
    client.Send(SVR_TRUE_RSP);
    __SVR_CURRENT_JOB = false;
}

void SvrHandling::StartPotFieldMode(SocketObject& client, string& args)
{
    __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;
        }
    }
    client.Send(SVR_TRUE_RSP);
}

void SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
{
    __POTFIELDMODE = false;

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

void SvrHandling::SetPos(SocketObject& client, string& args)
{

    string buf;
    double temp[15];
    stringstream ss(args);
    stringstream ss2f;
    vector<string> tokens;
    int i = 0;
    while (ss >> buf)
    {
        if (i>15-1)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        tokens.push_back(buf);
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> temp[i];
        i++;
    }
    if (i<15-1)
    {
        client.Send(SVR_FALSE_RSP);
        return ;
    }
    for (int i=0 ;i<3; i++)
    {
        for (int j=0;j<4;j++)
        {
            MSRCMDPOSE[i][j]=temp[i*4+j];
        }
    }
    float arg[3];
    for (int i=0;i<3;i++)
    {
        arg[i] = temp[12+i];
    }

    //Backward Calculation

    double* CalcJoints=NULL;
    CalcJoints = kukaBackwardCalc(temp, arg);

    //Check for Joint Range
    if (!checkJntRange(CalcJoints))
    {
        string out = "Error: Joints out of Range!";
        client.Send(out);
        return;
    }

    // Set global Position
    __MSRSETPOS = true;
    globi = 1;
    for (int i=0 ;i<LBR_MNJ; i++)
    {
        MSRCMDJNTPOS[i]=CalcJoints[i];
    }
    __MSRSETPOS = false;
    sleep(5);
    globi = 0;
    //__MSRCMDJNTPOS = true;

    ////Wait to end of movement
    //while (1)
    //{
    //  if (!__MSRCMDJNTPOS)
    //  {
    //      break;
    //  }
    //}
    client.Send(SVR_TRUE_RSP);
}

void SvrHandling::SetJoints(SocketObject& client, string& args)
{
    string buf;
    VecJoint newJointPos(0.0f);
    stringstream ss(args);
    stringstream ss2f;
    int i = 0;

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

        // convert string to float
        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 (!checkJntRange(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 cart pos
    SvrData::getInstance()->setCommandedJointPos(newJointPos);
    // TODO calculate and calculate position in Cartesian coordinates

    // wait till position is reached

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

void SvrHandling::MoveCartesian(SocketObject& client, string& args)
{
    __SVR_CURRENT_JOB = true;
    string buf;
    float temp[15];
    stringstream ss(args);
    stringstream ss2f;
    int i = 0;

    while (ss >> buf)
    {
        if (i>15-1)
        {
            client.Send(SVR_FALSE_RSP);
            return;
        }
        ss2f.flush();
        ss2f.clear();
        ss2f << buf;
        ss2f >> temp[i];
        i++;
    }
    if (i<15-1)
    {
        client.Send(SVR_FALSE_RSP);
        return ;
    }
    
    float arg[3];
    for (int i=0;i<3;i++)
    {
        arg[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);
    
    client.Send(SVR_TRUE_RSP);
    __DOUS2 = true;
}

void SvrHandling::quit(SocketObject& client)
{
    client.Send(SVR_TRUE_RSP);
    client.Disconnect();
}

void SvrHandling::debug(SocketObject& client)
{

    __DEBUG = true;
    
    //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);
    //}
}