#include "sgn.h"
#include <error.h>
#include <errno.h>
#include <math.h>
#include <Trajectroy.h>

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

#include "mat.h"
#include "vec.h"
#include "robot.h"

float* abctomat(float a, float b, float c)
{
    Mat4f rx;
    float ca = cos(c);
    float sa = sin(c);

    rx(0,0) = 1.0f;
    rx(1,1) = ca; 
    rx(1,2) = -sa;
    rx(2,1) = sa; 
    rx(2,2) = ca; 
    rx(3,3) = 1.0f;

    Mat4f ry;
    float cb = cos(b);
    float sb = sin(b);
    ry(0,0) = cb;
    ry(0,2) = sb;
    ry(1,1) = 1.0f;
    ry(2,0) = -sb;
    ry(2,2) = cb; 
    ry(3,3) = 1.0f;

    Mat4f rz;
    float cc = cos(a);
    float sc = sin(a);
    rz(0,0) = cc;
    rz(0,1) = -sc;
    rz(1,0) = sc;
    rz(1,1) = cc;
    rz(2,2) = 1.0f;
    rz(3,3) = 1.0f;

    Mat4f result;
    Mat4f temp;
    temp   = rz * ry;
    result = temp * rx;

#ifdef __DEBUG__
    printMat(rx);
    printMat(ry);
    printMat(rz);
    printMat(result);
#endif

    float *res = new float[12];
    //TODO simple converter for mat
    for (int j=0;j<3;j++)
    {
        for (int k=0;k<4;k++)
        {
            res[j*4+k] = result(j,k);
        }
    }
    return res;
}

float* mattoabc(float M[12])
{
    float norm;
    float sa;
    float ca;
    float *abc = new float[3];

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

    if (norm>1e-5)
    {
        sa = M[4]/norm;
        ca = M[0]/norm;
        abc[0]  = atan2(sa,ca);
    }
    else 
    {
        sa = 0;
        ca = 1;
        abc[0] = 0;
    }
    abc[1] = atan2(-M[8],ca*M[0]+sa*M[4]);
    abc[2] = atan2(sa*M[2]-ca*M[6],-sa*M[1]+ca*M[5]);
    return abc;
}

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

float* matToVec2(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;
}

float* vectoquat(float vec[12])
{
    float *quat = new float[4];
    float diag[3];
    int v; int w; int u;
    diag[0] = vec[0];
    diag[1] = vec[5];
    diag[2] = vec[10];
    float u_ = *std::max_element(diag,diag+2);

    if (u_==diag[0])
    {
        u = 1;
        v = 2;
        w = 3;
    } else if (u_==diag[1])
    {
        u = 2;
        v = 3;
        w = 1;
    } else
    {
        u = 3;
        v = 1;
        w = 2;
    }
    float r = sqrt(1+vec[(u-1)*4+(u-1)] - vec[(v-1)*4+(v-1)] - vec[(w-1)*4+(w-1)]);
    quat[0] = (vec[(w-1)*4+(v-1)] - vec[(v-1)*4+(w-1)]) / (2*r);
    quat[u] = r/2;
    quat[v] = (vec[(u-1)*4+(v-1)] + vec[(v-1)*4+(u-1)]) / (2*r);
    quat[w] = (vec[(w-1)*4+(u-1)] + vec[(u-1)*4+(w-1)]) / (2*r);

    return quat;
}

float* quattovec(float quat[4])
{
    float *vec = new float[12];
    vec[0] = quat[0]*quat[0]+quat[1]*quat[1]-quat[2]*quat[2]-quat[3]*quat[3];
    vec[4] = 2*(quat[1]*quat[2]+quat[0]*quat[3]);
    vec[8] = 2*(quat[1]*quat[3]-quat[0]*quat[2]);
    
    vec[1] = 2*(quat[1]*quat[2]-quat[0]*quat[3]);
    vec[5] = quat[0]*quat[0]-quat[1]*quat[1]+quat[2]*quat[2]-quat[3]*quat[3];
    vec[9] = 2*(quat[2]*quat[3]+quat[0]*quat[1]);
    
    vec[2] = 2*(quat[1]*quat[3]+quat[0]*quat[2]);
    vec[6] = 2*(quat[2]*quat[3]-quat[0]*quat[1]);
    vec[10] = quat[0]*quat[0]-quat[1]*quat[1]-quat[2]*quat[2]+quat[3]*quat[3];

    return vec;
}

void *threadRobotMovement(void *arg)
{
    // unused parameters 
    (void) arg;

    Trajectory<JOINT_NUMBER>* currentTrajectory = NULL;
    enum MovementType currentMovementType  = MovementJointBased;

    Robot::VecJoint currentJointPos(0.0f);
    MatCarthesian   currentCartPos(0.0f,1.0f);

    friRemote* friInst = SvrData::getInstance()->getFriRemote();

    // get current robot position
    int tries = 2;

    for (int i= 0 ; i < tries ; ++i)
    {
        if (REAL_ROBOT)
            SvrData::getInstance()->updateMessurement();
        currentJointPos = SvrData::getInstance()->getMeasuredJointPos();
    
        SvrData::getInstance()->setCommandedJointPos(currentJointPos);

        float newJointPosToSend[JOINT_NUMBER] = {0.0f};
        currentJointPos.getData(newJointPosToSend);
        if (REAL_ROBOT)
            friInst->doPositionControl(newJointPosToSend);
    }
    std::cout << "init position is " << currentJointPos << "\n";

    while(true)
    {
        if (REAL_ROBOT)
            SvrData::getInstance()->updateMessurement();

        bool positionChanged = false;
        // check if we have to cancel current trajectory
        bool cancel = SvrData::getInstance()->getTrajectoryCancel();
        if (cancel)
        {
            // free the current trajectory 
            if ( currentTrajectory != NULL)
            {
                delete currentTrajectory;
                currentTrajectory = NULL;
            }
            // signal waiting thread the cancellation
            SvrData::getInstance()->trajectoryCancelDone();
            goto end;
        }

        // check for new trajectory
        if (currentTrajectory == NULL)
        {
            // get next trajectory from svrData 
            currentTrajectory = SvrData::getInstance()->popTrajectory();

            // no new trajectory 
            if(currentTrajectory == NULL)
            {
                // mark that we reached the end of the trajectory
                // stay on current position
                goto end;
            }
            else
            {
                std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n";
            }
        }

        // get next commanded trajectory type
        if (currentTrajectory != NULL)
        {
            switch (currentTrajectory->getMovementType())
            {
                case MovementCartBased:
                {
                    currentCartPos = currentTrajectory->getNextCartPos();
                    positionChanged = true;
                    currentMovementType = MovementCartBased;
                }
                break;
                case MovementJointBased:
                {
                    currentJointPos = currentTrajectory->getNextJointPos();
                    positionChanged = true;
                    currentMovementType = MovementJointBased;
                }
                break;
                default:
                {
                    // invalid trajectory skip it
                    delete currentTrajectory;
                    currentTrajectory = NULL;
                    goto end;
                }
                break;
            }
        }

end:
        switch (currentMovementType)
        {
            case MovementJointBased:
                {
                    // send the new joint values
                    float newJointPosToSend[JOINT_NUMBER] = {0.0f};
                    Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI);
                    newJointPosToSendRad.getData(newJointPosToSend);
                    if (REAL_ROBOT)
                        friInst->doPositionControl(newJointPosToSend);
                }
                break;
            case MovementCartBased:
                {
                    float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0};
                    float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
                    float newCartPosToSend[12] = {0.0f};
                    currentCartPos.getData(newCartPosToSend);
                    if (REAL_ROBOT)
                        friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true);
                }
                break;
        }

        // start timer for the last sent msg
        // TODO 
        
        // check if current trajectory is over 
        if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0)
        {
            std::cout << " Trajectory finished \n ";

            static int trajcetorycount = 0;
            trajcetorycount +=1;

            currentTrajectory->saveToFile(std::string("trajectory/a.csv"));
            // invalid trajectory skip it
            delete currentTrajectory;
            currentTrajectory = NULL;
        }
    }
}

#if 0
void *threadFriDataExchange(void *arg)
{
    // unused 
    (void) arg;

    friRemote* friInst = SvrData::getInstance()->getFriRemote();

    while (1)
    {
        //#######################################
        // Communication loop

        // update current joint positions
        SvrData::getInstance()->updateMessurement();

        // get current joint position
        Robot::VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos();

        MatCarthesian currentCartPost = SvrData::getInstance()->getMessuredCartPos();

        // get current force and torque
        Robot::VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque();

        // get current jacobian
        // TODO use svr data
        float* friJacobian = friInst->getJacobian();
        if ( friJacobian == NULL)
        {
            fprintf(stderr,"Failed: could not get jacobian\n");
            break;
        }
        for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++)
        {
            MSRMSRJACOBIAN[i] = friJacobian[i];
        }

        //#########################################################
        // PTP Joint Movement
        if (__MSRCMDJNTPOS)
        {
            Vec<float,LBR_MNJ> maxJointLocalVelocity;
            Vec<float,LBR_MNJ> maxJointLocalAcceleration;
            Vec<float,LBR_MNJ> delta;
            Vec<float,LBR_MNJ> deltaAbs;
            Vec<float,LBR_MNJ> dMaxSpeed;
            Vec<float,LBR_MNJ> lMaxSpeed;
            Vec<float,LBR_MNJ> dGesamt;
            float sampleTime = SvrData::getInstance()->getSampleTimeMs();

            // get current robot constraints
            Vec<float,LBR_MNJ> maxVelocityJoint = SvrData::getInstance()->getMaxVelocity();
            Vec<float,LBR_MNJ> maxAccelarationJoint = SvrData::getInstance()->getMaxAcceleration();
            Vec<float,LBR_MNJ> messuredJointPos = SvrData::getInstance()->getMessuredJointPos();
            Vec<float,LBR_MNJ> commandedJointPos = SvrData::getInstance()->getCommandedJointPos();

            float velocity     = SvrData::getInstance()->getRobotVelocity();
            float accelaration = SvrData::getInstance()->getRobotAcceleration();

            // calculate delta positions of movement
            delta = commandedJointPos - messuredJointPos;
            deltaAbs = delta.abs();
            maxJointLocalVelocity = maxVelocityJoint * sampleTime * (velocity/100.0f);
            maxJointLocalAcceleration = maxAccelarationJoint * sampleTime * (accelaration/100.0f);

            // calculate number of movement steps
            dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
            lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;

            for (int j=0; j<LBR_MNJ; j++)
            {
                if (lMaxSpeed(j) > deltaAbs(j)/(double)2.0)
                {
                    dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0;
                } else 
                {
                    dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j);
                }
            }
            int maxSteps = ceil(dGesamt.max());

            // there are atleast 2 steps otherwise it is no trajectory
            if (maxSteps == 0 || maxSteps == 1)
                goto end;

            for (int j=0; j<LBR_MNJ; j++)
            {
                // ceil maxsteps/2 - sqrt(maxsteps^2 - deltaAbs/maxaccelaration)
                dMaxSpeed(j) = ceil( maxSteps /2.0f - sqrt((maxSteps/2.0f)*(maxSteps/2.0f) - deltaAbs(j)/maxAccelarationJoint(j)));
                if (dMaxSpeed(j) == 0.0f)
                {
                    maxJointLocalAcceleration(j) = 0.0f;
                } else
                {
                    maxJointLocalAcceleration(j) = -deltaAbs(j)/(dMaxSpeed(j)*dMaxSpeed(j)-maxSteps*dMaxSpeed(j));
                }
                maxJointLocalVelocity(j) = dMaxSpeed(j)*maxJointLocalAcceleration(j);
            }

            // do the trajectory
            {
                Vec<float,LBR_MNJ> currentInk;
                Vec<float,LBR_MNJ> currentInkLast;
                Vec<float,LBR_MNJ> currentDist;
                Vec<float,LBR_MNJ> currentDistLast;

                // bang bang
                for (int i=0;i<maxSteps;i++)
                {
                    for (int j=0;j<LBR_MNJ;j++)
                    {
                        if (i+1 <= maxSteps/2.0f)
                        {
                            currentInk(j) = min(currentInkLast(j)+maxJointLocalAcceleration(j),maxJointLocalVelocity(j));
                        }else if (i+1 > maxSteps-dMaxSpeed(j))
                        {
                            currentInk(j) = max(currentInkLast(j)-maxJointLocalAcceleration(j),0.0f);
                        }else
                        {
                            currentInk(j) = currentInkLast(j);
                        }
                        currentDist(j) = currentDistLast(j) + sgn(delta(j))*currentInk(j);
                        currentInkLast(j) = currentInk(j);
                        currentDistLast(j) = currentDist(j);
                        MSRMSRJNTPOS[j] += sgn(delta(j))*currentInk(j)*(1./180*M_PI);
                    }
                    // set new position
                    friInst->doPositionControl(MSRMSRJNTPOS);
                }
            }
end:
            // mark state to be done
            __MSRCMDJNTPOS = false;

        } else if (__CARTMOVE)
        {
            //##########################################################
            //Cartesian Movement

            const float Stiff[6] = {1000.0, 1000.0, 1000.0, 150.0, 150.0, 150.0};
            const float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
            float th = 0.000;
            float deltaCart[12];
            float Pos[12];
            float dist;
            float rot;
            float sum;
            float b;
            float c;
            float t_fact = 1;
            float t_outerT = 0.007;
            float t_innerT = 0.000;
            float t_outerR = 0.007;
            float t_innerR = 0.000;

            while (true)
            {
                for ( int i =0; i < FRI_CART_FRM_DIM; i++)
                {
                    Pos[i] = friInst->getMsrCartPosition()[i];
                    MSRMSRCARTPOS[i] = friInst->getMsrCartPosition()[i];
                    deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i];
                }
                sum = 0;
                for (int i=0; i<3 ; i++)
                {
                    sum = sum + (deltaCart[(i*4)+3]*deltaCart[(i*4)+3]);
                }
                dist = sqrt(sum);
                sum = 0;
                for (int i=0;i<12;i++)
                {
                    if (i != 3 && i != 7 && i != 11)
                    {
                        sum = sum + deltaCart[i]*deltaCart[i];
                    }
                }
                rot = sqrt(sum);
                b = t_fact*max((float)0.0,(float)min((float)1.0,(t_outerT-dist+(t_innerT-t_outerT))*(1/(t_innerT-t_outerT))));
                c = t_fact*max((float)0.0,(float)min((float)1.0,(float)(t_outerR-rot+(t_innerR-t_outerR))*(float)(1/(t_innerR-t_outerR))));

                if (dist < th)
                {
                    for (int i=0; i<3; i++)
                    {
                        deltaCart[(i*4)+3] = 0;
                    }
                }else 
                {
                    for (int i=0; i<3; i++)
                    {
                        deltaCart[(i*4)+3] = (deltaCart[(i*4)+3] / dist)*b;
                    }
                }

                // check offset to point 
                if (rot < th)
                {
                    for (int i=0;i<12;i++)
                    {
                        if (i != 3 && i != 7 && i != 11)
                        {
                            deltaCart[i] = 0;
                        }
                    }
                }else 
                {
                    for (int i=0;i<12;i++)
                    {
                        if (i != 3 && i != 7 && i != 11)
                        {
                            deltaCart[i] = (deltaCart[i] / rot)*c;
                        }
                    }
                }
                for (int i=0;i<12;i++)
                {
                    Pos[i]+= deltaCart[i]*0.02;
                }
                friInst->doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true);

            }
        } 
        //float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0};
        //float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
        //float Test[12] = {MSRMSRCARTPOS[0],MSRMSRCARTPOS[1],MSRMSRCARTPOS[2],MSRMSRCARTPOS[3],MSRMSRCARTPOS[4],MSRMSRCARTPOS[5],MSRMSRCARTPOS[6],MSRMSRCARTPOS[7],MSRMSRCARTPOS[8],MSRMSRCARTPOS[9],MSRMSRCARTPOS[10],MSRMSRCARTPOS[11]};
        //friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);

        // innerhlab 5ms
        friInst->doPositionControl(MSRMSRJNTPOS);

        //i = 0;
        //}
        //friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
    }
    return NULL;
}
#endif

int main()
{
    int err = 0;
    //Setting pthread for FRI interface
    pthread_t fri_t;
    //Start fri_thread
    err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL);
    if (err > 0 )
    {
        std::cerr << "Failed: could not create thread\n" << std::endl;
        return err;
    }

    //Start client handling
    SvrHandling *svr = new SvrHandling();
    if (svr == NULL)
    {
        std::cerr << "Failed: could not create server \n" << std::endl;
        return -ENOMEM;
    }
    svr->run();

    return 0;
}