 // getIndexOfLargestOpenInterval to get a free new direction
 // to explore.
 int findCylinder(){
-  //stepsInFullTurn;
-  //measurements;
   const int l = stepsInFullTurn;
 
-  while(1){
+  while(true){
     int nearestIndex = getIndexMin();
-
-    if(nearestIndex = -1){
+    if(nearestIndex == -1){
       return getIndexOfLargestOpenInterval();
     }
     

    }
  }
}

// Index of the closest measured distance
int getIndexMin(){
  int minVal = 500;   //Max
  int iMinVal = -1;
  
  for(int i=0; i<stepsInFullTurn; i++){
    if( !noCylinder[i] && (minVal > measurements[i]) ){
      iMinVal = i;
      minVal = measurements[i];
    }

    int iR = nextIndex(nearestIndex,l);
    int intervalLength = 0;
  
    while(abs(measurements[nearestIndex] - measurements[iL]) < 5){
      iL = prevIndex(iL,l);
      intervalLength++;
    }
    while(abs(measurements[nearestIndex] - measurements[iR]) < 5){
      iR = nextIndex(iR,l);
      intervalLength++;
    }

    if(intervalLength <= 5){
      return iL<iR ? (iL + iR)/2 : ((iL + iR+l)/2)%l;
    } else {
      for(int i=iL; i<iR; i++){
        noCylinder[i] = true; 
