#include <string>
#include <iostream>

#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>


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


void printArrr(double arr[], size_t size){
    cout<<"[";
    for(int i = 0; i < size; i++){
        cout<< arr[i] <<", ";
    }
    cout<<arr[size-1]<<"]"<<endl;
}

void getTouch(Port *p, double* buff, int &size){
    Bottle b;
    p->read(b);

    size = b.size();
    for(int i = 0; i < size; i++){
        buff[i] = b.get(i).asDouble();
    }
}

bool isNotZeros(double *arr, const int size){
    for (size_t i = 0; i < size; i++)
    {
        if(arr[i] != 0){
            return true;
        }
    }
    
    return false;
}

bool isArmTouch(Port *lh, Port *ph, Port *lf, Port *pf){
    double buff[600];
    int size;

    getTouch(lh, buff, size);
    if(isNotZeros(buff, size)){
        return true;
    }

    getTouch(ph, buff, size);
    if(isNotZeros(buff, size)){
        return true;
    }

    getTouch(lf, buff, size);
    if(isNotZeros(buff, size)){
        return true;
    }

    getTouch(pf, buff, size);
    if(isNotZeros(buff, size)){
        return true;
    }

    return false;
}



void leftHandPort(Port* port){
    string remoteHand = "/icubSim/skin/left_hand_comp";
    string localHand  = "/test/skin/touch/left_hand";
    port->open(localHand);
    Network::connect(remoteHand, localHand);
}

void rightHandPort(Port* port){
    string remoteHand = "/icubSim/skin/right_hand_comp";
    string localHand  = "/test/skin/touch/right_hand";
    port->open(localHand);
    Network::connect(remoteHand, localHand);
}

void leftForearmPort(Port* port){
    string remoteHand = "/icubSim/skin/left_forearm_comp";
    string localHand  = "/test/skin/touch/left_forearm";
    port->open(localHand);
    Network::connect(remoteHand, localHand);
}

void rightForearmPort(Port* port){
    string remoteHand = "/icubSim/skin/right_forearm_comp";
    string localHand  = "/test/skin/touch/right_forearm";
    port->open(localHand);
    Network::connect(remoteHand, localHand);
}




void leftHandPortClose(Port* port){
    string remoteHand = "/icubSim/skin/left_hand_comp";
    string localHand  = "/test/skin/touch/left_hand";
    Network::disconnect(remoteHand, localHand);
    //port->close(localHand);
}

void rightHandPortClose(Port* port){
    string remoteHand = "/icubSim/skin/right_hand_comp";
    string localHand  = "/test/skin/touch/right_hand";
    Network::disconnect(remoteHand, localHand);
    //port->close(localHand);
}

void leftForearmPortClose(Port* port){
    string remoteHand = "/icubSim/skin/left_forearm_comp";
    string localHand  = "/test/skin/touch/left_forearm";
    Network::disconnect(remoteHand, localHand);
    //port->close(localHand);
}

void rightForearmPortClose(Port* port){
    string remoteHand = "/icubSim/skin/right_forearm_comp";
    string localHand  = "/test/skin/touch/right_forearm";
    Network::disconnect(remoteHand, localHand);
    //port->close(localHand);
}