

#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <cstdio>
#include <stdio.h>
 
#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;


string getPositionData(string localPorts, string remotePorts){
    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", remotePorts);   //local port names
    options.put("remote", localPorts ); //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 "";
     }
     
     IEncoders *encs;
     bool ok = robotDevice.view(encs);
     if (!ok) {
         printf("Problems acquiring interfaces\n");
         return "";
     }

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

    double *a = new double[nj];
    for(int i=0; i<nj; i++){
        a[i] = 0;
    }

    // wait for encoders 
    while(!encs->getEncoders(a))
    {
     Time::delay(0.1);
    }
    
    stringstream ss;
    for(int i=0; i<nj; i++){
        ss << a[i];
        if(i+1 < nj){
            ss << " ";
        }
    }
    return ss.str();

}

string printPosition(Port p){
    stringstream ss;
    
    string remotePortsLeft = "/icubSim/left_arm";
    string localPortsLeft ="/test/clientl";
    string leftPos = getPositionData(remotePortsLeft, localPortsLeft);
    ss << "\"left-pos\": \"" << leftPos << "\"," << endl;

    string remotePortsRight = "/icubSim/right_arm";
    string localPortsRight ="/test/clientr";
    string rightPos = getPositionData(remotePortsRight, localPortsRight);
    ss << "\"right-pos\": \"" << rightPos << "\"";
    return ss.str();
}

string getSkinData(Port port, string inAddr, string outAddr){
    port.open(outAddr);
    Network::connect(inAddr, outAddr);
    Bottle b;
    port.read(b);
    Network::disconnect(inAddr, outAddr); 
    return b.toString().c_str();
}



string printSkin(Port p){
    stringstream ss;
    
    // left hand
    string remoteLeftHand = "/icubSim/skin/left_hand_comp";
    string localLeftHand ="/get/skin/data/left_hand";
    string resLeftHand = getSkinData(p, remoteLeftHand, localLeftHand);
    ss << "\"left-hand\" : \"" << resLeftHand << "\"," << endl;
    
    // right hand
    string remoteRightHand = "/icubSim/skin/right_hand_comp";
    string localRighHand ="/get/skin/data/right_hand";
    string resRightHand = getSkinData(p, remoteRightHand, localRighHand);
    ss << "\"right-hand\" : \"" << resRightHand << "\"," << endl;
    
    
    // left forehand
    string remoteLeftForeHand = "/icubSim/skin/left_forearm_comp";
    string localLeftForeHand ="/get/skin/data/left_forearm_comp";
    string resLeftForeHand = getSkinData(p, remoteLeftForeHand, localLeftForeHand);
    ss << "\"left-forearm\" : \"" << resLeftForeHand << "\"," << endl;    

    // right forehand
    string remoteRightForeHand = "/icubSim/skin/right_forearm_comp";
    string localRighForeHand ="/get/skin/data/right_forearm_comp";
    string resRightForeHand = getSkinData(p, remoteRightForeHand, localRighForeHand);
    ss << "\"right-forearm\" : \"" << resRightForeHand << "\"" << endl;
    return ss.str();
}

inline bool fileExists(const string& fileName){
    
    if(FILE *file = fopen(fileName.c_str(), "r")){
        fclose(file);
        return true;
    }else{
        return false;
    }
}

/**
 * Function for getting configuration and touches from iCub_SIM
 */
int main()
{
    Network yarp;
    if (!yarp.checkNetwork()){
        cout << "yarp is not running" << endl;
        return 1;
    }

    Port p;
    string pos = printPosition(p);
    string skin = printSkin(p);

    string fileName = "collected_data/test_notouch.rawdata";
    bool exist = fileExists(fileName);
    
    ofstream out;
    out.open(fileName, std::ios::app);
    if(!exist){
        out << "[\n";
    }else{
        out << ",\n";
    }
    
    out << "{\n";
    out << pos;
    out << ",\n";
    out << skin;
    out << "}\n";
    out.close();
    return 0;
}
