     }
 
      if (c > C_HOLE_CLEAR) {
-          // Check if there was enough data to tell the skittle's color
-          if(min_clear < C_SKITTLE_CLEAR){
-              skittleCount ++;
-              Serial.println("A Skittle has been detected!");
-              best_color.print();
+
               best_color.maximize();
-              Serial.println("Maximized colors:");
-              best_color.print();
-              if(F_CALIBRATING){
-                __calibrating(best_color.r, best_color.g, best_color.b);
+              if(best_color.compare(colorList[0]).aggregate() < 16){
+                
+                        Serial.println("Empty hole");
+                
+              }else{
+                        skittleCount ++;
+                        Serial.println("A Skittle has been detected!");
+                        Serial.println("Maximized colors:");
+                        best_color.print();
+
+
+                              if(F_CALIBRATING){
+                                __calibrating(best_color);
+                              }else{
+                                    
+                                    int min_diff = 64;
+                                    int result = -1;
+                                    for(int i = 0; i < sizeof(colorList)/8; i++){
+                                      C_Color diff = best_color.compare(colorList[i]);
+                                      int agg = diff.aggregate();
+                                      if(agg < min_diff){
+                                        min_diff = agg;
+                                        result = i;
+                                      }
+                                    }
+                                    switch(result){
+                                      case 0:
+                                        Serial.println("Empty hole");
+                                        break;
+                                      case 1:
+                                        Serial.println("RED");
+                                        break;
+                                      case 2:
+                                        Serial.println("GREEN");
+                                        break;
+                                      case 3:
+                                        Serial.println("YELLOW");
+                                        break;
+                                      case 4:
+                                        Serial.println("PURPLE");
+                                        break;
+                                      case 5:
+                                        Serial.println("ORANGE");
+                                        break;
+                                      default:
+                                        Serial.println("Unknown color");
+                                        break;
+                                    }
+                                      
+                              }
+                
               }
-              Serial.println("Diff:");
-              C_Color diff = best_color.compare(C_SKITTLE_GREEN);
-              diff.print();
-              Serial.println( diff.aggregate());
-
-          }
-          else{
-              // Serial.println("The hole is empty!!");
-              // printColors(best_r, best_g, best_b, min_clear);
-          }
 
           // Serial.println("Finish measuring.");
           isColorBeingMeasured = false;

#define C_ATIME TCS34725_INTEGRATIONTIME_101MS //  Color sensor integration time 50ms
#define C_CYCLES (256-C_ATIME) // Color sensor cycles according to the integration time

#define C_HOLE_CLEAR 256 // The clear value used to determine the hole's arrival
#define C_SKITTLE_CLEAR 160 // The clear value used to determine the Skittle's arrival

#define F_CALIBRATING false // Is calibration in progress


  servoBtm.write(btmAngle);
}

void __calibrating(C_Color new_color){
  /* This function is used to test and get the average color 
   *  of a certain Skittle
  */
  static C_Color avg_c = C_Color(new_color.r, new_color.g, new_color.b);
  static int count = 0;
  int diff = avg_c.compare(new_color).aggregate();
  Serial.print("Diff: "); Serial.println(diff);
  if(diff > 64){
    Serial.println("Difference too big. This data is discarded.");
  }else{
    avg_c.r = (avg_c.r * count + new_color.r) / (count + 1);
    avg_c.g = (avg_c.g * count + new_color.g) / (count + 1);
    avg_c.b = (avg_c.b * count + new_color.b) / (count + 1);
    count++;
    Serial.println("Average color:");
    avg_c.print();
  }
  
  
}
