void ReactiveLayer::configureTactile(yarp::os::ResourceFinder &rf)
{
    //Initialise the tactile response
    Bottle grpTactile = rf.findGroup("TACTILE");
    Bottle *tactileStimulus = grpTactile.find("stimuli").asList();

    if (tactileStimulus)
    {
        for (int d = 0; d<tactileStimulus->size(); d++)
        {
            string tactileStimulusName = tactileStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpTactile.find((tactileStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            //choregraphies
            Bottle *bChore = grpTactile.find((tactileStimulusName + "-chore").c_str()).asList();
            for (int sC = 0; bChore && sC<bChore->size(); sC++)
            {
                response.m_choregraphies.push_back(bChore->get(sC).asString().c_str());
            }
            std::string sGroupTemp = tactileStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpTactile.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            tactileEffects[tactileStimulusName] = response;
        }
    }
}

void AdaptiveLayer::configureGestures(yarp::os::ResourceFinder &rf)
{
    //Initialise the gestures response
    Bottle grpGesture = rf.findGroup("GESTURES");
    Bottle *gestureStimulus = grpGesture.find("stimuli").asList();

    if (gestureStimulus)
    {
        for(int d=0; d<gestureStimulus->size(); d++)
        {
            string gestureStimulusName = gestureStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpGesture.find((gestureStimulusName + "-sentence").c_str()).asList();
            for(int s=0;s<bSentences->size();s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = gestureStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpGesture.find( sGroupTemp.c_str()).asList();
            for(int i=0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i+1).asDouble();
            }
            gestureEffects[gestureStimulusName] = response;
        }
    }
}

void ReactiveLayer::configureSalutation(yarp::os::ResourceFinder &rf)
{
    ;
    //Initialise the gestures response
    Bottle grpSocial = rf.findGroup("SOCIAL");
    salutationLifetime = grpSocial.check("salutationLifetime", Value(15.0)).asDouble();
    Bottle *socialStimulus = grpSocial.find("stimuli").asList();

    if (socialStimulus)
    {
        for (int d = 0; d<socialStimulus->size(); d++)
        {
            string socialStimulusName = socialStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpSocial.find((socialStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = socialStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpSocial.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            salutationEffects[socialStimulusName] = response;
        }
    }

    //Add the relevant Entities for handling salutation
    iCub->opc->addOrRetrieveAction("is");
    iCub->opc->addOrRetrieveAdjective("saluted");
}

bool visualFilterModule::configure(yarp::os::ResourceFinder &rf) {
    /* Process all parameters from both command-line and .ini file */

    /* get the module name which will form the stem of all module port names */
    moduleName            = rf.check("name", 
                           Value("/visualFeaX"), 
                           "module name (string)").asString();
    /*
    * before continuing, set the module name before getting any other parameters, 
    * specifically the port names which are dependent on the module name
    */
    setName(moduleName.c_str());

    /*
    * get the robot name which will form the stem of the robot ports names
    * and append the specific part and device required
    */
    robotName             = rf.check("robot", 
                           Value("icub"), 
                           "Robot name (string)").asString();
    robotPortName         = "/" + robotName + "/head";

    /*
    * attach a port of the same name as the module (prefixed with a /) to the module
    * so that messages received from the port are redirected to the respond method
    */
    handlerPortName =  "";
    handlerPortName += getName();         // use getName() rather than a literal 

    if (!handlerPort.open(handlerPortName.c_str())) {           
        cout << getName() << ": Unable to open port " << handlerPortName << endl;  
        return false;
    }

    attach(handlerPort);                  // attach to port

    /* create the thread and pass pointers to the module parameters */
    printf("trying to start the main thread \n");
    vfThread = new visualFilterThread();
    vfThread->setName(getName().c_str());
    
    /* now start the thread to do the work */
    vfThread->start(); // this calls threadInit() and it if returns true, it then calls run()

    return true ;       // let the RFModule know everything went well
                        // so that it will then run the module
}

bool ReactiveLayer::configure(yarp::os::ResourceFinder &rf)
{
    string moduleName = rf.check("name",Value("ReactiveLayer")).asString().c_str();
    setName(moduleName.c_str());

    cout<<moduleName<<": finding configuration files..."<<endl;
    period = rf.check("period",Value(0.1)).asDouble();

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = false;
    iCub = new ICubClient(moduleName,"reactiveLayer","client.ini",isRFVerbose);
    iCub->opc->isVerbose = false;
    char rep = 'n';
    while (rep!='y'&&!iCub->connect())
    {
        cout<<"iCubClient : Some dependencies are not running..."<<endl;
        //cout<<"Continue? y,n"<<endl;
        //cin>>rep;
        break; //to debug
        Time::delay(1.0);
    }

    //Set the voice
    std::string ttsOptions = rf.check("ttsOptions", yarp::os::Value("iCubina 85.0")).asString();
    if (iCub->getSpeechClient())
        iCub->getSpeechClient()->SetOptions(ttsOptions);

    //Configure the various components
    configureOPC(rf);
    configureAllostatic(rf);
    configureTactile(rf);
    configureSalutation(rf);

    cout<<"Configuration done."<<endl;

    rpc.open ( ("/"+moduleName+"/rpc").c_str());
    attach(rpc);

    //Initialise timers
    lastFaceUpdate = Time::now();
    physicalInteraction = false;
    someonePresent = false;

    //iCub->getReactableClient()->SendOSC(yarp::os::Bottle("/event reactable pong start"));

    return true;
}

bool GuiUpdaterModule::configure(yarp::os::ResourceFinder &rf)
{
    iCub = NULL;

    string moduleName      = rf.check("name", 
        Value("guiUpdater"), 
        "module name (string)").asString().c_str();

    string opcName      = rf.check("OPCname", 
        Value("OPC"), 
        "OPC name (string)").asString().c_str();

    setName(moduleName.c_str());

    displaySkeleton = rf.check("displaySkeletons");
    cout<<"Display skeleton status: "<<displaySkeleton<<endl;

    w = new OPCClient(getName().c_str());
    w->connect(opcName);
    w->isVerbose = false;

    //GUI Port
    string guiPortName = "/";
    guiPortName +=  getName() + "/gui:o";
    toGui.open(guiPortName.c_str());
    resetGUI();

    //GUI Base Port
    string guiBasePortName = "/";
    guiBasePortName +=  getName() + "/guiBase:o";
    toGuiBase.open(guiBasePortName.c_str());

    //RPC
    string handlerPortName = "/";
    handlerPortName +=  getName() + "/rpc";
    if (!handlerPort.open(handlerPortName.c_str())) {           
        cout << getName() << ": Unable to open port " << handlerPortName << endl;  
        return false;
    }
    attach(handlerPort);                  // attach to port

    return true ;
}

bool lumaChroma::configure(yarp::os::ResourceFinder &rf)
{    
    /* Process all parameters from both command-line and .ini file */

    moduleName          = rf.check("name", 
                        Value("lumaChroma"), 
                        "module name (string)").asString();

    setName(moduleName.c_str());

    imageType           = rf.check("image", 
                        Value("yuv"), 
                        "image type (string)").asString();

    whichPort           = rf.check("out",
                        Value(""),
                        "default port (string)").asString();

   /*
    * attach a port of the same name as the module (prefixed with a /) to the module
    * so that messages received from the port are redirected to the respond method
    */

    handlerPortName =  "/";
    handlerPortName += getName();         // use getName() rather than a literal 
 
    if (!handlerPort.open(handlerPortName.c_str())) 
    {           
        cout << getName() << ": Unable to open port " << handlerPortName << endl;  
        return false;
    }

    attach(handlerPort);    // attach to port

    /* create the thread and pass pointers to the module parameters */
    procThread = new PROCThread(moduleName, imageType, whichPort);

    /* now start the thread to do the work */
    procThread->open();

    return true ;      // let the RFModule know everything went well
}

bool actionClass::parseTorqueBalancingSequences(std::string              filenamePrefix,
        std::string              filenameSuffix,
        int                      partID,
        std::string              format,
        yarp::os::ResourceFinder &rf)
{
    bool ret = false;
    actionStructForTorqueBalancing tmp_action;
    string filename  = filenamePrefix + "_" + filenameSuffix + ".txt";
    filename  = rf.findFile(filename);
    fprintf(stderr, "[!!!] File found for %s: %s\n", filenameSuffix.c_str(), filename.c_str());
    // Open file
    ifstream data_file( filename.c_str() );
    string line;
    std::deque<double> tmp_com;
    std::deque<double> tmp_postural;
    std::deque<double> tmp_constraints;

    while( std::getline(data_file, line) )
    {
        int col = 0;
        std::istringstream iss( line );
        std::string result;
        tmp_com.clear();
        tmp_postural.clear();
        tmp_constraints.clear();
        while( std::getline( iss, result , ' ') )
        {
            if ( strcmp(result.c_str(),"") != 0 )
            {
                std::stringstream convertor;
                convertor.clear();
                convertor.str(result);
                if (partID == COM_ID)
                    tmp_com.push_back(atof(result.c_str()));
                if (partID == POSTURAL_ID)
                    tmp_postural.push_back(atof(result.c_str()));
                if (partID == CONSTRAINTS_ID)
                    tmp_constraints.push_back(atoi(result.c_str()));
                col++;
            }
        }
        if (partID == COM_ID)
            action_vector_torqueBalancing.com_traj.push_back(tmp_com);
        if (partID == POSTURAL_ID)
            action_vector_torqueBalancing.postural_traj.push_back(tmp_postural);
        if (partID == CONSTRAINTS_ID)
            action_vector_torqueBalancing.constraints.push_back(tmp_constraints);
    }

    data_file.close();
    ret = true;
    return ret;
}

bool ShowModule::configure(yarp::os::ResourceFinder &rf)
{
    //Ports
    string name=rf.check("name",Value("show3D")).asString().c_str();
    string robot = rf.check("robot",Value("icub")).asString().c_str();
    string cloudpath_file = rf.check("from",Value("cloudsPath.ini")).asString().c_str();
    rf.findFile(cloudpath_file.c_str());

    ResourceFinder cloudsRF;    
    cloudsRF.setDefaultConfigFile(cloudpath_file.c_str());
    cloudsRF.configure(0,NULL);

    // Set the path that contains previously saved pointclouds
    if (rf.check("clouds_path")){
        cloudpath = rf.find("clouds_path").asString().c_str();
    }else{
        string defPathFrom = "/share/ICUBcontrib/contexts/toolIncorporation/sampleClouds/";
        string localModelsPath    = rf.check("local_path")?rf.find("local_path").asString().c_str():defPathFrom;     //cloudsRF.find("clouds_path").asString();
        string icubContribEnvPath = yarp::os::getenv("ICUBcontrib_DIR");
        cloudpath  = icubContribEnvPath + localModelsPath;
    }

    handlerPort.open("/"+name+"/rpc:i");
        attach(handlerPort);

    cloudsInPort.open("/"+name+"/clouds:i");

    // Module rpc parameters
    closing = false;

    // Init variables
    cloudfile = "cloud.ply";

    cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr (new pcl::PointCloud<pcl::PointXYZRGB>);

    //Threads
    visThrd = new VisThread(50, "Cloud");
    if (!visThrd->start())
    {
        delete visThrd;
        visThrd = 0;
        cout << "\nERROR!!! visThread wasn't instantiated!!\n";
        return false;
    }
    cout << "PCL visualizer Thread istantiated...\n";
    cout << endl << "Configuring done."<<endl;

    printf("Base path: %s \n \n",cloudpath.c_str());

    return true;
}

void printInstalledFolders(yarp::os::ResourceFinder &rf, folderType ftype)
{
    ResourceFinderOptions opts;
    opts.searchLocations=ResourceFinderOptions::Installed;
    yarp::os::Bottle contextPaths=rf.findPaths(getFolderStringName(ftype), opts);
    printf("**INSTALLED DATA:\n");
    for (int curPathId=0; curPathId<contextPaths.size(); ++curPathId)
    {
        printf("* Directory : %s\n", contextPaths.get(curPathId).asString().c_str());
        printContentDirs(contextPaths.get(curPathId).asString());
    }
}

    /* 
    * Configure function. Receive a previously initialized
    * resource finder object. Use it to configure your module.
    * Open port and attach it to message handler.
    */
    bool configure(yarp::os::ResourceFinder &rf)
    {
        timeout=rf.check("timeout",Value(60.0)).asDouble();
        vergencePort.open("/radHelper/vergence:i");
        handlerPort.open("/radHelper/attentionStatus:io");
        suspendPort.open("/radHelper/suspend:o");
        attach(handlerPort);

        //Network::connect("/gazeArbiter/icub/left_cam/status:o","/radHelper/vergence:i","udp");
        //Network::connect("/radHelper/suspend:o","/gazeArbiter/icub/left_cam");
        return true;
    }

bool InterpersonalDistanceRegulator::configure(yarp::os::ResourceFinder &rf)
{
	isPaused = false;
	isQuitting = false;

    string moduleName = rf.check("name",Value("interpersonalDistanceRegulator")).asString().c_str();
    setName(moduleName.c_str());

    cout<<moduleName<<": finding configuration files..."<<endl;
    period = rf.check("period",Value(0.1)).asDouble();
	preferedDistanceToPeople = rf.check("preferedDistanceToPeople",Value(0.9)).asDouble();
	fwdSpeed = rf.check("forwardSpeed", Value(0.075)).asDouble();
	backwdSpeed = rf.check("backwardSpeed", Value(0.075)).asDouble();
	turningSpeed = rf.check("angularSpeed", Value(0.075)).asDouble();

	ikart = new SubSystem_iKart(moduleName);
	while (!ikart->Connect())
	{
		cout << "Connecting IDR to iKart..." << endl;
		Time::delay(1.0);
	}

	opc = new OPCClient(moduleName);
	while (!opc->connect("OPC"))
	{
		cout << "Connecting InterpersonalThread to OPC..." << endl;
		Time::delay(1.0);
	}
    cout<<"Configuration done."<<endl;

    rpc.open ( ("/"+moduleName+"/rpc").c_str());
    attach(rpc);
    return true;
}

void prepareHomeFolder(yarp::os::ResourceFinder &rf, folderType ftype)
{
    ACE_DIR* dir= YARP_opendir((rf.getDataHome()).c_str());
    if (dir!=NULL)
        YARP_closedir(dir);
    else
    {
        yarp::os::mkdir((rf.getDataHome()).c_str());
    }

    dir= YARP_opendir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringName(ftype)).c_str());
    if (dir!=NULL)
        YARP_closedir(dir);
    else
    {
        yarp::os::mkdir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringName(ftype)).c_str());
    }
    dir= YARP_opendir((rf.getDataHome() + PATH_SEPARATOR + getFolderStringNameHidden(ftype)).c_str());
    if (dir!=NULL)
        YARP_closedir(dir);
    else
    {
        ConstString hiddenPath=(rf.getDataHome() + PATH_SEPARATOR + getFolderStringNameHidden(ftype));
        yarp::os::mkdir(hiddenPath.c_str());
#ifdef WIN32
        SetFileAttributes(hiddenPath.c_str(), FILE_ATTRIBUTE_HIDDEN);
#endif
    }
}

bool QuaternionEKF::readEstimatorParams(yarp::os::ResourceFinder &rf, quaternionEKFParams &estimatorParams)
{
    yarp::os::Bottle botParams;
    botParams = rf.findGroup("QuaternionEKF");
    yInfo("QuaternionEKF params are: %s ", botParams.toString().c_str());
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from QuaternionEKF group");
        return false;
    } else {
        estimatorParams.stateSize = botParams.find("state_size").asInt();
        estimatorParams.inputSize = botParams.find("input_size").asInt();
        estimatorParams.measurementSize = botParams.find("measurement_size").asInt();
        estimatorParams.muSystemNoise = botParams.find("mu_system_noise").asDouble();
        estimatorParams.sigmaSystemNoise = botParams.find("sigma_system_noise").asDouble();
        estimatorParams.sigmaMeasurementNoise = botParams.find("sigma_measurement_noise").asDouble();
        estimatorParams.sigmaGyro = botParams.find("sigma_gyro_noise").asDouble();
        estimatorParams.piorMu = botParams.find("prior_mu_state").asDouble();
        estimatorParams.priorCovariance = botParams.find("prior_cov_state").asDouble();
        estimatorParams.muGyroNoise = botParams.find("mu_gyro_noise").asDouble();
        estimatorParams.floatingBaseAttitude = botParams.find("floating_base_attitude").asBool();
        estimatorParams.rot_from_ft_to_acc_bottle = new yarp::os::Bottle(*botParams.find("rot_from_ft_to_acc").asList());
    }

    botParams.clear();
    botParams = rf.findGroup("module_parameters");
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from 'module_params' group. period is needed!");
        return false;
    } else {
        estimatorParams.period = botParams.find("period").asInt();
        estimatorParams.robotPrefix = botParams.find("robot").asString();
        estimatorParams.streamMeasurements = botParams.find("stream_measurements").asBool();
    }
    return true;
}

bool ears::configure(yarp::os::ResourceFinder &rf)
{
    string moduleName = rf.check("name", Value("ears")).asString().c_str();
    setName(moduleName.c_str());

    yInfo() << moduleName << " : finding configuration files...";
    period = rf.check("period", Value(0.1)).asDouble();

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = false;
    iCub = new ICubClient(moduleName, "ears", "client.ini", isRFVerbose);
    iCub->opc->isVerbose = false;
    if (!iCub->connect())
    {
        yInfo() << " iCubClient : Some dependencies are not running...";
        Time::delay(1.0);
    }
    rpc.open(("/" + moduleName + "/rpc").c_str());
    attach(rpc);

    portToBehavior.open("/" + moduleName + "/behavior:o");
    while (!Network::connect(portToBehavior.getName(),"/BehaviorManager/trigger:i ")) {
        yWarning() << " Behavior is not reachable";
        yarp::os::Time::delay(0.5);
    }

    portTarget.open("/" + moduleName + "/target:o");

    MainGrammar = rf.findFileByName(rf.check("MainGrammar", Value("MainGrammar.xml")).toString());

    bShouldListen = true;

    yInfo() << "\n \n" << "----------------------------------------------" << "\n \n" << moduleName << " ready ! \n \n ";
    
    return true;
}

bool AudioPowerMapModule::configure(yarp::os::ResourceFinder &rf) {

	yInfo("Configuring the module");

	// get the module name which will form the stem of all module port names 
	moduleName = rf.check("name", Value("/AudioPowerMap"), "module name (string)").asString();


	// before continuing, set the module name before getting any other parameters, 
	// specifically the port names which are dependent on the module name
	setName(moduleName.c_str());


	// get the robot name which will form the stem of the robot ports names
	// and append the specific part and device required
	robotName     = rf.check("robot", Value("icub"), "Robot name (string)").asString();
	robotPortName =  "/" + robotName + "/head";
	inputPortName =  rf.check("inputPortName", Value(":i"),	"Input port name (string)").asString();


	// attach a port of the same name as the module (prefixed with a /) to the module
	// so that messages received from the port are redirected to the respond method
	handlerPortName =  "";
	handlerPortName += getName();         

	if (!handlerPort.open(handlerPortName.c_str())) {           
		std::cout << getName() << ": Unable to open port " << handlerPortName << std::endl;  
		return false;
	}

	// attach to port
	attach(handlerPort);                  
	if (rf.check("config")) {
		configFile=rf.findFile(rf.find("config").asString().c_str());
		if (configFile=="") {
			return false;
		}
	}

	else {
		configFile.clear();
	}

	// create the thread and pass pointers to the module parameters
	apr = new AudioPowerMapRatethread(robotName, configFile, rf);
	apr->setName(moduleName);

	// now start the thread to do the work 
	// this calls threadInit() and it if returns true, it then calls run()
	bool ret = apr->start(); 

	// let the RFModule know everything went well
	// so that it will then run the module
	return ret;         
}

    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();
        
        mNavThread=new Navigator(&rf);

        if (!mNavThread->start())
        {
            delete mNavThread;
            return false;
        }

        std::string local=rf.check("local",yarp::os::Value("/ikartnav")).asString().c_str();
        mHandlerPort.open(local.c_str());
        attach(mHandlerPort);
        //attachTerminal();

        return true;
    }

 bool configure( yarp::os::ResourceFinder &rf )
   {
       std::string moduleName = rf.check("name", 
               yarp::os::Value("demoServerModule"), 
               "module name (string)").asString().c_str();
       setName(moduleName.c_str());
       
       std::string slash="/";
       
       attach(cmdPort);
       
       std::string cmdPortName= "/";
       cmdPortName+= getName();
       cmdPortName += "/cmd";
       if (!cmdPort.open(cmdPortName.c_str())) {           
           std::cout << getName() << ": Unable to open port " << cmdPortName << std::endl;  
           return false;
       }
       return true;
   }   

    bool configure(yarp::os::ResourceFinder &rf)
    {
	path = rf.find("clouds_path").asString();        
	printf("Path: %s",path.c_str());		
	handlerPort.open("/mergeModule");
        attach(handlerPort);

	/* Module rpc parameters */
	visualizing = false;
	saving = true;
        closing = false;

	/*Init variables*/
	initF = true;	
	filesScanned = 0;

	cout<<"Configuring done."<<endl;

        return true;
    }

bool Recognition::initPorts(yarp::os::ResourceFinder &rf){
	moduleName            = rf.check("name", 
                           Value("Recognition"), 
                           "module name (string)").asString();
        /*
    * before continuing, set the module name before getting any other parameters, 
    * specifically the port names which are dependent on the module name
    */
    setName(moduleName.c_str());

    // Open cam left
	camLeftName = "/";
    camLeftName += getName() + "/in/left";

    if (!camLeft.open(camLeftName.c_str())) {           
        cout << getName() << ": Unable to open port " << camLeftName << endl;  
        return false;
    }
   
	portAckName = "/";
    portAckName += getName() + "/out";

    if (!portAck.open(portAckName.c_str())) {           
        cout << getName() << ": Unable to open port " << portAckName << endl;  
        return false;
    }
   
	portInName = "/";
    portInName += getName() + "/in";

    if (!portIn.open(portInName.c_str())) {           
        cout << getName() << ": Unable to open port " << portInName << endl;  
        return false;
    }
    attach(portIn);
    
    return true;
}