#include <string>
#include <iostream>

#include <yarp/os/Time.h>
#include <yarp/os/Network.h>
#include <yarp/os/Port.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/Value.h>
#include <yarp/sig/Vector.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/PolyDriver.h>

#include "touchLib.h"

using namespace std;
using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;


void maxSpeed(PolyDriver* robotDevice){
    cout<<"speeding iCub"<<endl;

    IPositionControl *pos;
    IEncoders *encs;
    
    bool ok;
    ok = robotDevice->view(pos);
    ok = ok && robotDevice->view(encs);

    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return;
    }

    int nj=0;
    pos->getAxes(&nj);
    Vector encoders;
    Vector command;
    Vector tmp;
    encoders.resize(nj);
    tmp.resize(nj);
    command.resize(nj);

    int i;
    for (i = 0; i < nj; i++) {
        tmp[i] = 50.0;
    }
    pos->setRefAccelerations(tmp.data());

    for (i = 0; i < nj; i++) {
        tmp[i] = 80.0;
        pos->setRefSpeed(i, tmp[i]);
    }
    pos->setRefSpeeds(tmp.data());
}


PolyDriver* makeMoveDriver(string remotePort, string localPort){
    cout<<"making driver for "<<remotePort<<" on port "<<localPort<<endl;
    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", localPort);   //local port names
    options.put("remote", remotePort); //where we connect to
 
    // create a device
    PolyDriver* robotDevice = new PolyDriver(options);
    if (!robotDevice->isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return robotDevice;
    }
    return robotDevice;
}

void getArmPos(PolyDriver* robotDevice, double* buff){
    IEncoders *encs;
    bool ok = robotDevice->view(encs);
    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return;
    }

    int nj=0;
    encs->getAxes(&nj);

    while(!encs->getEncoders(buff))
    {
        Time::delay(0.1);
    }
}

bool myMoveArm(PolyDriver* robotDevice, Port* lht, Port* rht, Port* lat, Port* rat, double angles[], size_t size){
    cout<<"moveing hand "<< angles[0]<<" "<< angles[1]<<" " << angles[2]<<" " << angles[3]<<" " << angles[4]<<" " << angles[5]<<" " << endl;
    IPositionControl *pos;
    IEncoders *encs;

    bool ok;
    ok = robotDevice->view(pos);
    ok = ok && robotDevice->view(encs);
    
    int nj=0;
    pos->getAxes(&nj);
    Vector encoders;
    Vector command;
    encoders.resize(nj);
    command.resize(nj);

    // fisrst read all encoders
    while(!encs->getEncoders(encoders.data()))
    {
        Time::delay(0.1);
    }
    command=encoders;

    // set new position
    for(int i = 0; i < size; i++){
        command[i] = angles[i];
    }

    // move
    bool done=false;
    pos->positionMove(command.data());
    if(isArmTouch(lht, rht, lat, rat)){
            Time::delay(0.5);
    }
    while(!done)
    {
        if(isArmTouch(lht, rht, lat, rat)){
            printf("--Dotyk\n");
            pos->stop();
            return true;        
        }else{
            printf("--Nedotyk\n");        
        }
        pos->checkMotionDone(&done);
        Time::delay(0.2);
    }
    return false;
}


