string MCDMFunction::getEncodedKey(Pose& p, int value)
{
    string key;
    //value = 0 : encode everything
    //value = 1 : encode x,y,orientation, take first 
    //value = 2 : encode x,y,orientation, take multiple time
    if(value == 0){
	key =  to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/"  + to_string(p.getRange()) + "/" + to_string(p.getFOV());
    }else if(value == 1){
	key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/" + "1";
    } else if (value ==2){
	key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/" + "0";
    }
    return key;
}
geometry_msgs::Pose MessageConversions::poseToMessage(const Pose& pose)
    const {
  geometry_msgs::Pose message;
  
  const Eigen::Vector3d& position = pose.getPosition();
  message.position.x = position[0];
  message.position.y = position[1];
  message.position.z = position[2];
  
  const Eigen::Quaterniond& orientation = pose.getOrientation();
  message.orientation.x = orientation.x();
  message.orientation.y = orientation.y();
  message.orientation.z = orientation.z();
  message.orientation.w = orientation.w();
  
  return message;
}
pair<Pose,double> MCDMFunction::selectNewPose(EvaluationRecords *evaluationRecords)
{    
    Pose newTarget;
    double value = 0;
    unordered_map<string,double> evaluation = evaluationRecords->getEvaluations();
    for(unordered_map<string,double>::iterator it = evaluation.begin(); it != evaluation.end(); it++){
	string tmp = (*it).first;
	Pose p = evaluationRecords->getPoseFromEncoding(tmp);
	if(value <= (*it).second){
		newTarget = p;
		value = (*it).second;
	    }//else continue;
    }
    pair<Pose,double> result = make_pair(newTarget,value);
    
    // i switch x and y to allow debugging graphically looking the image
    cout << "New target : " << "x = "<<newTarget.getY() <<", y = "<< newTarget.getX() << ", orientation = " 
	    <<newTarget.getOrientation() << ", Evaluation: "<< value << endl;
    return result;
}
// Input : ./mcdm_online_exploration_ros ./../Maps/map_RiccardoFreiburg_1m2.pgm 100 75 5 0 15 180 0.95 0.12
// resolution x y orientation range centralAngle precision threshold
int main ( int argc, char **argv )
{
  auto startMCDM = chrono::high_resolution_clock::now();
  ifstream infile;
  infile.open ( argv[1] );  // the path to the map
  double resolution = atof ( argv[2] );  // the resolution of the map
  double imgresolution = atof ( argv[10] );  // the resolution to use for the planningGrid and RFIDGrid
  dummy::Map map = dummy::Map ( infile,resolution, imgresolution );
  RFIDGridmap myGrid(argv[1], resolution, imgresolution, false);
  cout << "Map dimension: " << map.getNumGridCols() << " : "<<  map.getNumGridRows() << endl;
  int gridToPathGridScale = map.getGridToPathGridScale();
  // i switched x and y because the map's orientation inside and outside programs are different
  long initX = static_cast<long>( atoi ( argv[4] ) *imgresolution );  // initial X-position of the robot in map frame
  long initY = static_cast<long>( atoi ( argv[3] ) *imgresolution );  // initial Y-position of the robot in map frame
  std::cout << "initX: " << initX << " initY: " << initY << std::endl;
  int initOrientation = atoi ( argv[5] );  // initial orientation of the robot in map frame
  double initFov = atoi ( argv[7] );  // initial FOV of the robot sensor
  initFov = initFov * PI /180;
  int initRange = atoi ( argv[6] );
  double precision = atof ( argv[8] );
  double threshold = atof ( argv[9] );
  //x,y,orientation,range,FOV

  Pose initialPose = Pose ( initX,initY,initOrientation,initRange,initFov );
  Pose invertedInitial = createFromInitialPose ( initX,initY,initOrientation,180,initRange,initFov );
  Pose eastInitial = createFromInitialPose ( initX,initY,initOrientation,90,initRange,initFov );
  Pose westInitial = createFromInitialPose ( initX,initY,initOrientation,270,initRange,initFov );
  Pose target = initialPose;
  Pose previous = initialPose;
  long numConfiguration = 1;
  vector<pair<string,list<Pose>>> graph2;
  NewRay ray;
  ray.setGridToPathGridScale ( gridToPathGridScale );
  MCDMFunction function;
  long sensedCells = 0;
  long newSensedCells =0;
  long totalFreeCells = map.getTotalFreeCells();
  int count = 0;
  double travelledDistance = 0;
  int numOfTurning = 0;
  unordered_map<string,int> visitedCell;
  vector<string>history;
  history.push_back ( function.getEncodedKey ( target,1 ) );
  EvaluationRecords record;
  //amount of time the robot should do nothing for scanning the environment ( final value expressed in second)
  unsigned int microseconds = 5 * 1000 * 1000 ;
  list<Pose> unexploredFrontiers;
  list<Pose> tabuList;
  tabuList.push_back(target);
  list<Pose> nearCandidates;
  bool btMode = false;
  double totalAngle = 0;
  Astar astar;
  double totalScanTime = 0;
  bool act = true;
  int encodedKeyValue = 0;

  // RFID
  double absTagX =  std::stod(argv[12]); // m.
  double absTagY =  std::stod(argv[11]); // m.
  double freq = std::stod(argv[13]); // Hertzs
  double txtPower= std::stod(argv[14]); // dBs
  std::pair<int, int> relTagCoord;

  do
  {
    // If we are doing "forward" navigation towards cells never visited before
    if ( btMode == false )
    {
      long x = target.getX();
      long y = target.getY();
      int orientation = target.getOrientation();
      int range = target.getRange();
      double FOV = target.getFOV();
      string actualPose = function.getEncodedKey ( target,0 );
      map.setCurrentPose ( target );
      string encoding = to_string ( target.getX() ) + to_string ( target.getY() );
      visitedCell.emplace ( encoding,0 );
      // Get the sensing time required for scanning
      target.setScanAngles ( ray.getSensingTime ( map,x,y,orientation,FOV,range ) );
      // Perform a scanning operation
      newSensedCells = sensedCells + ray.performSensingOperation ( map,x,y,orientation,FOV,range, target.getScanAngles().first, target.getScanAngles().second );
      // Calculate the scanning angle
      double scanAngle = target.getScanAngles().second - target.getScanAngles().first;
      // Update the overall scanning time
      totalScanTime += calculateScanTime ( scanAngle*180/PI );
      // Calculare the relative RFID tag position to the robot position
      relTagCoord = map.getRelativeTagCoord(absTagX, absTagY, target.getX(), target.getY());
      // Calculate the received power and phase
      double rxPower = received_power_friis(relTagCoord.first, relTagCoord.second, freq, txtPower);
      double phase = phaseDifference(relTagCoord.first, relTagCoord.second, freq);
      // Update the path planning and RFID map
      map.updatePathPlanningGrid ( x, y, range, rxPower - SENSITIVITY);
      myGrid.addEllipse(rxPower - SENSITIVITY, map.getNumGridCols() - target.getX(),  target.getY(), target.getOrientation(), -0.5, 7.0);
      // Search for new candidate position
      ray.findCandidatePositions ( map,x,y,orientation,FOV,range );
      vector<pair<long,long> >candidatePosition = ray.getCandidatePositions();
      ray.emptyCandidatePositions();

      // Old code
      // TODO: leave it, I don't know what it does
//      if ( act )
//      {
//        //NOTE; calculate path and turnings between actual position and goal
//        /*string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
//              travelledDistance = travelledDistance + astar.lengthPath ( path );
//              cout << astar.lengthPath ( path ) << endl;
//              numOfTurning = numOfTurning + astar.getNumberOfTurning ( path );
//              numConfiguration++;
//              */
//        //totalAngle += scanAngle;
//      }
//      act = false;

      // If the exploration just started
      if ( count == 0 )
      {
        // Calculate other three pose given the strating one
        string invertedPose = function.getEncodedKey ( invertedInitial,0 );
        string eastPose = function.getEncodedKey ( eastInitial,0 );
        string westPose = function.getEncodedKey ( westInitial,0 );
        list<Pose> empty ;
        std::pair<string,list<Pose>> pair1 = make_pair ( invertedPose,empty );
        std::pair<string,list<Pose>> pair2 = make_pair ( eastPose,empty );
        std::pair<string,list<Pose>> pair3 = make_pair ( westPose,empty );
        // And add them (with empty candidates) to the graph structure
        graph2.push_back ( pair1 );
        graph2.push_back ( pair2 );
        graph2.push_back ( pair3 );
      }

      // If it's not the first step but we are in one of the initial position (we come back here with backtracking)
      if ( count != 0 && ( target.isEqual ( invertedInitial ) || target.isEqual ( eastInitial ) || target.isEqual ( westInitial ) ) )
      {
        // If there are no more destination in the graph, terminates the navigation
        if ( graph2.size() == 0 ) break;
        graph2.pop_back();
        actualPose = function.getEncodedKey ( target,0 );
        // Add to the graph the initial positions and the candidates from there (calculated inside the function)
        pushInitialPositions ( map, x, y,orientation, range,FOV, threshold, actualPose, &graph2 );
      }


      // If there are no new candidate positions from the current pose of the robot
      if ( candidatePosition.size() == 0 )
      {
        // Find candidates
        ray.findCandidatePositions2 ( map,x,y,orientation,FOV,range );
        candidatePosition = ray.getCandidatePositions();
        ray.emptyCandidatePositions();

        cout << "No other candidate position" << endl;
        cout << "----- BACKTRACKING -----" << endl;
        // If the graph contains cells that can be explored
        if ( graph2.size() > 1 )
        {
          // Get the last position in the graph and then remove it
          string targetString = graph2.at ( graph2.size()-1 ).first;
          graph2.pop_back();
//          EvaluationRecords record;
          target = record.getPoseFromEncoding ( targetString );
          // Add it to the history as cell visited more than once
          history.push_back ( function.getEncodedKey ( target,2 ) );
          cout << "[BT]No significative position reachable. Come back to previous position" << endl;
          count = count + 1;
        }
        //...otherwise, if the graph does not contain cells that can be explored
        // The navigation is finished!
        else
        {
          cout << "Num configuration: " << numConfiguration << endl;
          cout << "Travelled distance calculated during the algorithm: " << travelledDistance << endl;
          cout << "------------------ HISTORY -----------------" << endl;
          // Retrieve the cell visited only the first time
          list<Pose> tmp_history = cleanHistory(&history, &record);
          calculateDistance(tmp_history, map, &astar );

          cout << "------------------ TABULIST -----------------" << endl;
          // Calculate the path connecting the cells in the tabulist, namely the cells that are visited one time and couldn't be visite again
          calculateDistance(tabuList, map, &astar );

          // Normalise the travel distance in meter
          // NOTE: assuming that the robot is moving at 0.5m/s and the resolution of the map is 0.5m per cell)
          if ( imgresolution == 1.0 )
          {
            travelledDistance = travelledDistance/2;
          }
          printResult(newSensedCells, totalFreeCells, precision, numConfiguration, travelledDistance, numOfTurning,
              totalAngle, totalScanTime);
          exit ( 0 );
        }

        sensedCells = newSensedCells;

      }
      //... otherwise, if there are further candidate new position from the current pose of the robot
      else
      {
        // need to convert from a <int,int pair> to a Pose with also orientation,laser range and angle
        list<Pose> frontiers;
        // For every candidate positio, create 8 pose with a different orientation each and consider them as frontiers
        vector<pair<long,long> >::iterator it =candidatePosition.begin();
        for ( it; it != candidatePosition.end(); it++ )
        {
          Pose p1 = Pose ( ( *it ).first, ( *it ).second,0 ,range,FOV );
          Pose p2 = Pose ( ( *it ).first, ( *it ).second,45,range,FOV );
          Pose p3 = Pose ( ( *it ).first, ( *it ).second,90,range,FOV );
          Pose p4 = Pose ( ( *it ).first, ( *it ).second,135,range,FOV );
          Pose p5 = Pose ( ( *it ).first, ( *it ).second,180,range,FOV );
          Pose p6 = Pose ( ( *it ).first, ( *it ).second,225,range,FOV );
          Pose p7 = Pose ( ( *it ).first, ( *it ).second,270,range,FOV );
          Pose p8 = Pose ( ( *it ).first, ( *it ).second,315,range,FOV );
          frontiers.push_back ( p1 );
          frontiers.push_back(p2);
          frontiers.push_back ( p3 );
          frontiers.push_back(p4);
          frontiers.push_back ( p5 );
          frontiers.push_back(p6);
          frontiers.push_back ( p7 );
          frontiers.push_back(p8);

        }

        unexploredFrontiers = frontiers;

        // Evaluate the frontiers and return a list of <frontier, evaluation> pairs
        EvaluationRecords *record = function.evaluateFrontiers ( frontiers,map,threshold );
        nearCandidates = record->getFrontiers();

        // If there are candidate positions
        if ( record->size() != 0 )
        {
          // Set the previous pose equal to the current one (represented by target)
          previous = target;
          // Select the new robot destination from the list of candidates
          std::pair<Pose,double> result = function.selectNewPose ( record );
          target = result.first;
          // If the selected destination does not appear among the cells already visited
          if ( ! contains ( tabuList,target ))
          {
            act = true;
            // Add it to the list of visited cells as first-view
            encodedKeyValue = 1;
            updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function,
                &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle);
//                  count = count + 1;
//                  // 			numConfiguration++;
//                  // 			cout << "ADDED" << x << " " << y << endl;
//                  // Add it to the list of visited cells as first-view
//                  history.push_back ( function.getEncodedKey ( target,1 ) );
//                  cout << function.getEncodedKey ( target,1 ) << endl;
//                  // Add it to the list of visited cells from which acting
//                  tabuList.push_back ( target );
//                  // Remove it from the list of candidate position
//                  cleanPossibleDestination2 ( nearCandidates,target );
//                  // Push in the graph the previous robot pose and the new list of candidate position, without the current pose of the robot
//                  // We don't want to visit this cell again
//                  std::pair<string,list<Pose>> pair = make_pair ( actualPose,nearCandidates );
//                  graph2.push_back ( pair );
//                  // Calculate the path from the previous robot pose to the current one
//                  string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
//                  cout << "1: " << travelledDistance << endl;
//                  // Update the distance counting
//                  travelledDistance = travelledDistance + astar.lengthPath(path);
//                  cout << "2: " << travelledDistance << endl;
//                  // Update the turning counting
//                  numOfTurning = numOfTurning + astar.getNumberOfTurning(path);
//                  // Update the number of configurations of the robot along the task
//                  numConfiguration++;
//                  // Update the scanned angle counting
//                  totalAngle += scanAngle;
          }
          // ...otherwise, if the seleced cell has already been visited
          else
          {
            // If the graph is empty, stop the navigation
            if ( graph2.size() == 0 ) break;
            // If there still are more candidates to explore from the last pose in the graph
            if ( graph2.at ( graph2.size()-1 ).second.size() != 0 )
            {
              cout << "[BT1 - Tabulist]There are visible cells but the selected one is already explored!Come back to second best position from the previous position"<< endl;
              // Remove the current position from possible candidates
              cleanPossibleDestination2 ( nearCandidates,target );
              // Get the list of new candidate position with associated evaluation
              record = function.evaluateFrontiers ( nearCandidates,map,threshold );
              // If there are candidate positions
              if ( record->size() != 0 )
              {
                // Select the new pose of the robot
                std::pair<Pose,double> result = function.selectNewPose ( record );
                target = result.first;
                encodedKeyValue = 1;
                updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function,
                                  &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle);
//                        // Add it to the list of visited cell to not go back
//                        tabuList.push_back ( target );
//                        // Add it to history of cell as visited for the first time
//                        history.push_back ( function.getEncodedKey ( target,1 ) );
//                        cout << function.getEncodedKey ( target,1 ) << endl;
//                        // Calculate the path from current robot pose to there
//                        string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
//                        cout << "1: " << travelledDistance << endl;
//                        // Update the overall distance covered by the robot
//                        travelledDistance = travelledDistance + astar.lengthPath(path);
//                        cout << "2: " << travelledDistance << endl;
//                        // Update the overall number of turnings
//                        numOfTurning = numOfTurning + astar.getNumberOfTurning(path);
//                        // Update the number of configurations of the robot during the navigation
//                        numConfiguration++;
//                        // Update the overall scan angle
//                        totalAngle += scanAngle;
//                        count = count + 1;
                // Set that we are now in backtracking
                btMode = true;
              }
              // If there are no more candidate position from the last position in the graph
              else
              {
                // if the graph is now empty, stop the navigation
                if ( graph2.size() == 0 ) break;
                // Otherwise, select as new position the last cell in the graph and then remove it from there
                string targetString = graph2.at ( graph2.size()-1 ).first;
                graph2.pop_back();
                target = record->getPoseFromEncoding ( targetString );
              }
            }
            // ... if the graph still does not present anymore candidate positions for its last pose
            else
            {
              // Remove the last element (cell and associated candidate from there) from the graph
              graph2.pop_back();
              // Select as new target, the new last element of the graph
              string targetString = graph2.at ( graph2.size()-1 ).first;
              target = record->getPoseFromEncoding ( targetString );
              // Save it history as cell visited more than once
              history.push_back ( function.getEncodedKey ( target,2 ) );
              cout << "[BT2 - Tabulist]There are visible cells but the selected one is already explored!Come back to two position ago"<< endl;
              count = count + 1;
            }

          }
        }
        // ... otherwise, if there are no candidate positions
        else
        {
          // If the graph is empty, stop the navigation
          if ( graph2.size() == 0 ) break;
          // Select as new target the last one in the graph structure
          string targetString = graph2.at ( graph2.size()-1 ).first;
          // Remove it from the graph
          graph2.pop_back();
          target = record->getPoseFromEncoding ( targetString );
          // Check if the selected cell in the graph is the previous robot position
          if ( !target.isEqual ( previous ) )
          {
            // if it's not, set the old position as the current one
            previous = target;  //TODO: WHY?
            cout << "[BT3]There are no visible cells so come back to previous position in the graph structure" << endl;
            // Save the new target in the history as cell visited more than once
            history.push_back ( function.getEncodedKey ( target,2 ) );
            count = count + 1;
          }
          // If the selected cell is the old robot position
          else
          {
            // If there are no more cells in the graph, just finish the navigation
            if ( graph2.size() == 0 )
            {
              cout << "[BT4]No other possibilities to do backtracking on previous positions" << endl;
              break;
            }
            // Select the last position in the graph
            string targetString = graph2.at ( graph2.size()-1 ).first;
            // and remove it from the graph
            graph2.pop_back();
            target = record->getPoseFromEncoding ( targetString );
            // Set the previous pose as the current one
            previous = target;
            cout << "[BT5]There are no visible cells so come back to previous position" << endl;
            cout << "[BT5]Cell already explored!Come back to previous position"<< endl;
            // Add it in history as cell visited more than once
            history.push_back ( function.getEncodedKey ( target,2 ) );
            count = count + 1;
          }

        }



        //NOTE: not requested for testing purpose
        //usleep(microseconds);
        sensedCells = newSensedCells;
        frontiers.clear();
        candidatePosition.clear();
        delete record;
      }

    }
    // ... otherwise, if we are doing backtracking
    else
    {
      long x = target.getX();
      long y = target.getY();
      int orientation = target.getOrientation();
      int range = target.getRange();
      double FOV = target.getFOV();
      string actualPose = function.getEncodedKey ( target,0 );
      map.setCurrentPose ( target );
      //NOTE; calculate path and turnings between actual position and goal
      cout<< function.getEncodedKey ( target,1 ) << endl;
      // Calculate the distance between the previous robot pose and the next one (target)
      string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
      // Update the overall covered distance
      travelledDistance = travelledDistance + astar.lengthPath ( path );
      cout << "BT: " << astar.lengthPath ( path ) << endl;
      // Update the overall number of turnings
      numOfTurning = numOfTurning + astar.getNumberOfTurning ( path );

      string encoding = to_string ( target.getX() ) + to_string ( target.getY() );
      visitedCell.emplace ( encoding,0 );
      // Set the previous cell to be the same of the current one
      previous = target;

      // Calculate how much time it takes to scan the current area
      target.setScanAngles ( ray.getSensingTime ( map,x,y,orientation,FOV,range ) );
      // Get the scanning angle
      double scanAngle = target.getScanAngles().second - target.getScanAngles().first;
      // Update the overall scanned angle
      totalAngle += scanAngle;
      // ...and the overall scan time
      totalScanTime += calculateScanTime ( scanAngle*180/PI );
      // Calculate the relative coordinate to the robot of the RFID tag
      relTagCoord = map.getRelativeTagCoord(absTagX, absTagY, target.getX(), target.getY());
      // Calculate received power and phase
      double rxPower = received_power_friis(relTagCoord.first, relTagCoord.second, freq, txtPower);
      double phase = phaseDifference(relTagCoord.first, relTagCoord.second, freq);
      map.updatePathPlanningGrid ( x, y, range, rxPower - SENSITIVITY );
      myGrid.addEllipse(rxPower - SENSITIVITY, map.getNumGridCols() - target.getX(), target.getY(), target.getOrientation(), -0.5, 7.0);
      // Remove the current pose from the list of possible candidate cells
      cleanPossibleDestination2 ( nearCandidates,target );
      // Get the list of the candidate cells with their evaluation
      EvaluationRecords *record = function.evaluateFrontiers ( nearCandidates,map,threshold );

      // If there are candidate cells
      if ( record->size() != 0 )
      {
        // Find the new destination
        std::pair<Pose,double> result = function.selectNewPose ( record );
        target = result.first;
        // If this cells has not been visited before
        if ( ! contains ( tabuList,target ) )
        {
          // Add it to the list of visited cells as first-view
          encodedKeyValue = 1;
          updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function,
                            &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle);
//
//                // Add it to history as visited for the first time
//                history.push_back ( function.getEncodedKey ( target,1 ) );
//                // Add it to cell that must not be visited again
//                tabuList.push_back ( target );
//                cout<< function.getEncodedKey ( target,1 ) << endl;
//                // Calculate the distance between previous pose to the new one
//                string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
//                // Update the overall counting of distance
//                travelledDistance = travelledDistance + astar.lengthPath(path);
//                cout << "BTL "<<astar.lengthPath ( path ) << endl;
//                // Update the counting of turning
//                numOfTurning = numOfTurning + astar.getNumberOfTurning(path);
//                // Update the counting of configuration of the robot during the navigation
//                numConfiguration++;
//                // Update the overall scanned angle
//                totalAngle += scanAngle;
//                // Remove the destination from the list of candidate cells
//                cleanPossibleDestination2 ( nearCandidates,target );
//                // Associate the list of candiates to the current pose
//                std::pair<string,list<Pose>> pair = make_pair ( actualPose,nearCandidates );
//                count = count + 1;

          // Leave the backtracking branch
          btMode = false;
          nearCandidates.clear();
          cout << "[BT-MODE4] Go back to previous positions in the graph" << endl;
        }
        // ... otherwise, if the cells has already been visisted
        else
        {
          // If there are other candidates
          if ( nearCandidates.size() != 0 )
          {
            cout << "[BT-MODE1]Already visited, but there are other candidates" << endl;

            // Remove the destination from the candidate list
            cleanPossibleDestination2 ( nearCandidates,target );
            // Get the candidates with their evaluation
            EvaluationRecords *record = function.evaluateFrontiers ( nearCandidates,map,threshold );
            // Select the new destination
            std::pair<Pose,double> result = function.selectNewPose ( record );
            target = result.first;

            // Add it to the list of visited cells as first-view
            encodedKeyValue = 1;
            updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function,
                              &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle);
//                    // Add it to the list of cell that must not be visited again
//                    tabuList.push_back ( target );
//                    history.push_back ( function.getEncodedKey ( target,1 ) );  // added recently but it was not here
//                    cout<< function.getEncodedKey ( target,1 ) << endl;
//                    // Calculate the distance between the destination and the previous pose
//                    string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
//                    cout << "BT :"<<astar.lengthPath ( path ) << endl;
//                    // Update the overall distance covered by the robot
//                    travelledDistance = travelledDistance + astar.lengthPath(path);
//                    // Update the overall number of turnings
//                    numOfTurning = numOfTurning + astar.getNumberOfTurning(path);
//                    // Update the number of configuration of the robot during navigation
//                    numConfiguration++;
//                    // Update the overall scanned angle
//                    totalAngle += scanAngle;
//                    count = count + 1;
          }
          // ...otherwise, if there are no more candidates
          else
          {
            cout << "[BT-MODE2] Go back to previous positions in the graph" << endl;
            // Select as target the last element in the graph
            string targetString = graph2.at ( graph2.size()-1 ).first;
            // And remove from the graph
            graph2.pop_back();
            target = record->getPoseFromEncoding ( targetString );
            // Add it to the history of cell as already more than once
            encodedKeyValue = 2;
            updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function,
                              &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle);
//                    // Add it to the list of cells that must not be visited again
//                    tabuList.push_back ( target );
//                    // Add it to the history of cell as already more than once
//                    history.push_back ( function.getEncodedKey ( target,2 ) );
//                    // Calculate the distance from the previous pose and the destination
//                    string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
//                    cout <<"BT: "<< astar.lengthPath ( path ) << endl;
//                    // Update the overall distance of the robot
//                    travelledDistance = travelledDistance + astar.lengthPath(path);
//                    // Update the overall number of turnings
//                    numOfTurning = numOfTurning + astar.getNumberOfTurning(path);
//                    // Update the overall number of configuration of the robot during navigation
//                    numConfiguration++;
//                    // Update the overall scanned angle
//                    totalAngle += scanAngle;
//                    count++;
            // Leave backtracking
            btMode = false;
            // Clear candidate list
            nearCandidates.clear();
          }
        }
      }
      // ... if there are not candidate cells
      else
      {
        // Select as new pose, the last cell in the graph
        string targetString = graph2.at ( graph2.size()-1 ).first;
        // and the remove it form the graph
        graph2.pop_back();
        target = record->getPoseFromEncoding ( targetString );

        // Add it in history as cell visited more than once
        encodedKeyValue = 2;
        updatePathMetrics(&count, &target, &previous, actualPose, &nearCandidates, &graph2, &map, &function,
                          &tabuList, &history, encodedKeyValue, &astar, &numConfiguration, &totalAngle, &travelledDistance, &numOfTurning, scanAngle);
//                  // Add it to the list of cell that must not be visited again
//                  tabuList.push_back ( target );
//                  // Add it in history as cell visited more than once
//                  history.push_back ( function.getEncodedKey ( target,2 ) );
//                  // Calculate the distance from the previous cell to the new destination
//                  string path = astar.pathFind ( target.getX(),target.getY(),previous.getX(),previous.getY(),map );
//                  cout <<"BT:"<< astar.lengthPath ( path ) << endl;
//                  // Update the overall distance covered by the robot
//                  travelledDistance = travelledDistance + astar.lengthPath(path);
//                  // Update the oevrall number of turning of the robot
//                  numOfTurning = numOfTurning + astar.getNumberOfTurning(path);
//                  // Update the number of configuration of the robot during navigation
//                  numConfiguration++;
//                  // Update the overall scanned angle
//                  totalAngle += scanAngle;
//                  count = count + 1;
        // Leave backtracking
        btMode = false;
        cout << "[BT-MODE3] Go back to previous positions in the graph" << endl;
      }
      delete record;
    }
  }
  // Perform exploration until a certain coverage is achieved
  while ( sensedCells < precision * totalFreeCells );
  // Plotting utilities
  map.drawVisitedCells ();
  map.printVisitedCells ( history );
  map.drawRFIDScan();
  map.drawRFIDGridScan(myGrid);
  myGrid.saveAs(("/home/pulver/Desktop/MCDM/rfid_result_gridmap.pgm"));

  cout << "Num configuration: " << numConfiguration << endl;
  cout << "Travelled distance calculated during the algorithm: " << travelledDistance << endl;

  cout << "------------------ HISTORY -----------------" << endl;
  // Calculate which cells have been visited only once
  list<Pose> tmp_history = cleanHistory(&history, &record);
  calculateDistance(tmp_history, map, &astar );

  cout << "------------------ TABULIST -----------------" << endl;
  calculateDistance(tabuList, map, &astar );

  // Trasform distance in meters
  if ( imgresolution == 1.0 ) // Corridor map has a resolution of 0.5 meter per cell
  {
    travelledDistance = travelledDistance/2;
  }


  printResult(newSensedCells, totalFreeCells, precision, numConfiguration, travelledDistance, numOfTurning,
      totalAngle, totalScanTime);
  // Find the tag
  std::pair<int,int> tag = map.findTag();
  cout << "RFID pose: [" << tag.second << "," << tag.first << "]" << endl;
  tag = map.findTagfromGridMap(myGrid);
  cout << "[Grid]RFID pose: [" << tag.second << "," << tag.first << "]" << endl;
  cout << "-----------------------------------------------------------------"<<endl;
  auto endMCDM= chrono::high_resolution_clock::now();

  double totalTimeMCDM = chrono::duration<double,milli> ( endMCDM -startMCDM ).count();
  //     cout << "Total time for MCDM algorithm : " << totalTimeMCDM << "ms, " << totalTimeMCDM/1000 <<" s, " <<
  // 		totalTimeMCDM/60000 << " m "<< endl;

}
string EvaluationRecords::getEncodedKey(Pose& p)
{
    string key =  to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( (int)p.getOrientation()) + "/" + to_string(p.getRange()) +"/" + to_string(p.getFOV());
    return key;
}