 #include <string>
 #include <iostream>
 #include <iomanip>
 #include <cstdio>
 
#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/math/Math.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/PolyDriver.h>

#include <iCub/iKin/iKinVocabs.h>
#include <iCub/iKin/iKinHlp.h>
#include <iCub/iKin/iKinSlv.h>
 
using namespace std;
using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace yarp::math;
using namespace iCub::iKin;
 

double * getAngles(Bottle *b, int size){
    double * res = new double[size];
    for(int i = size-1; i != 0; i--){
    //for(int i = 0; i < size; i++){
        res[i] = b -> pop().asDouble();
    }
    return res;
}



void move(double* angles, int size){
     std::string remotePorts = "/icubSim/left_arm";
 
     std::string localPorts="/test/client";
 
     Property options;
     options.put("device", "remote_controlboard");
     options.put("local", localPorts);   //local port names
     options.put("remote", remotePorts); //where we connect to
 
     // create a device
     PolyDriver robotDevice(options);
     if (!robotDevice.isValid()) {
         printf("Device not available.  Here are the known devices:\n");
         printf("%s", Drivers::factory().toString().c_str());
         return;
     }
 
     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] = 100.0;
         pos->setRefSpeed(i, tmp[i]);
     }
 
     pos->setRefSpeeds(tmp.data());
     
     //fisrst read all encoders
     //
     printf("waiting for encoders");
     while(!encs->getEncoders(encoders.data()))
     {
         Time::delay(0.1);
         printf(".");
     }
     printf("\n");
     command=encoders;
     //now set the shoulder to some value
     for(int i = 0; i < size; i++){
        command[i] = angles[i];
     }
     pos->positionMove(command.data());
     
     bool done=false;
 
     while(!done)
     {
         pos->checkMotionDone(&done);
         Time::delay(0.1);
     }
 
  robotDevice.close();
}


 
 int main()
 {
     Bottle cmd, reply;
     Network yarp;
 
     if (!yarp.checkNetwork())
         return 1;
     
     // declare the on-line arm solver called "solver"
     iCubArmCartesianSolver onlineSolver("solver");
 
     Property options;
     // it will operate on the simulator (which is supposed to be already running)
     options.put("robot","icubSim");
     // it will work with the right arm
     options.put("type","right");
     // it will achieve just the positional pose
     options.put("pose","xyz");
     // switch off verbosity
     options.put("verbosity","off");
 
     // launch the solver and let it connect to the simulator
     if (!onlineSolver.open(options))
         return 1;
     
     // prepare ports
     Port in, out, rpc;
     in.open("/in"); out.open("/out"); rpc.open("/rpc");
     Network::connect("/solver/out",in.getName());
     Network::connect(out.getName(),"/solver/in");
     Network::connect(rpc.getName(),"/solver/rpc");
 
     // print status    
     cmd.clear();
     cmd.addVocab(IKINSLV_VOCAB_CMD_GET);
     cmd.addVocab(IKINSLV_VOCAB_OPT_DOF);
     rpc.write(cmd,reply);
     cout<<"got dof: "<<reply.toString()<<endl;
 
     cmd.clear();
     cmd.addVocab(IKINSLV_VOCAB_CMD_GET);
     cmd.addVocab(IKINSLV_VOCAB_OPT_POSE);
     rpc.write(cmd,reply);
     cout<<"got pose: "<<reply.toString()<<endl;
 
     cmd.clear();
     cmd.addVocab(IKINSLV_VOCAB_CMD_GET);
     cmd.addVocab(IKINSLV_VOCAB_OPT_MODE);
     rpc.write(cmd,reply);
     cout<<"got mode: "<<reply.toString()<<endl;
 
     // change to tracking mode so that when
     // any movement induced on unactuated joints
     // is detected the solver is able to react
     cmd.clear();
     cmd.addVocab(IKINSLV_VOCAB_CMD_SET);
     cmd.addVocab(IKINSLV_VOCAB_OPT_MODE);
     cmd.addVocab(IKINSLV_VOCAB_VAL_MODE_TRACK);
     cout<<"switching to track mode...";
     rpc.write(cmd,reply);
     cout<<reply.toString()<<endl;
 
     // ask to resolve for some xyz position
     cmd.clear();
     Vector xd(3);
        cout<<"x = ";
        cin>>xd[0];
        cout<<"y = ";
        cin>>xd[1];
        cout<<"z = ";
        cin>>xd[2];
     CartesianHelper::addTargetOption(cmd,xd);
     out.write(cmd);
     in.read(reply);
 
     cout<<"xd      ="<<CartesianHelper::getTargetOption(reply)->toString()<<endl;
     cout<<"x       ="<<CartesianHelper::getEndEffectorPoseOption(reply)->toString()<<endl;
     cout<<"q [deg] ="<<CartesianHelper::getJointsOption(reply)->toString()<<endl;
     cout<<endl;
    
    Bottle * bla = CartesianHelper::getJointsOption(reply);
    double * angles = getAngles(bla, 7);
    move(angles, 7);
 
     // ask the same but with torso enabled
//     Vector dof(3,1.0);
//     CartesianHelper::addDOFOption(cmd,dof);
//     out.write(cmd);
//     in.read(reply);
 
//     cout<<"xd      ="<<CartesianHelper::getTargetOption(reply)->toString()<<endl;
//     cout<<"x       ="<<CartesianHelper::getEndEffectorPoseOption(reply)->toString()<<endl;
//     cout<<"q [deg] ="<<CartesianHelper::getJointsOption(reply)->toString()<<endl;
//     cout<<endl;
 
     // close up
     onlineSolver.close();
     in.close();
     out.close();
     rpc.close();
 
     return 0;
 }
 
