     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);
+    avg_c.c = (avg_c.c * count + new_color.c) / (count + 1);
     count++;
     Serial.println("Average color:");
     avg_c.print();
   }
 }
 
+void analyzeColor(C_Color best_color) {
+  // How many times have tried to detect the color
+  static int retried = 0;
+  if (!F_CALI_EMPTY_HOLE && best_color.compare(C_COLOR_EMPTY).aggregate() < 16
+      && abs(int(best_color.c) - int(C_COLOR_EMPTY.c)) < 32) {
+    // If this is an empty hole.
+    Serial.println("Empty hole");
+    if (isRemeasuring) {
+      isRemeasuring = false;
+      remeasuringReverseDirection = false;
+      retried = 0;
+    }
+  } else {
+    Serial.println("A Skittle has been detected!");
+    //Serial.println("Maximized colors:");
+    //best_color.print();
+
+#if F_CALIBRATING
+    // If we are calibrating colors
+    __calibrating(best_color);
+#else
+    // Compare the reported color with the pre-defined colors in the colorList
+    // and set the result to the color with the least color variance, if the
+    // variance is still under the allowed variance range (C_ALLOWED_COLOR_VARIANCE)
+
+    int min_diff = C_ALLOWED_COLOR_VARIANCE; // Set the mininum difference to the allowed color variance
+    int temp_result = -1;
+    for (int i = 0; i < sizeof(colorList) / 8; i++) {
+      // Compare the reported color with the color defined in the colorList
+      C_Color diff = best_color.compare(colorList[i]);
+
+      // Add the color difference's primary colors (R + G + B) together
+      int agg = diff.aggregate();
+      // Check if the aggregated color difference value is less than the minimum color difference
+      if (agg < min_diff) {
+        // If this is less than the minimun
+        min_diff = agg; // Set the minimun difference to this aggregated color difference
+        temp_result = i; // Set the result to this color's index
+      }
+    }
+
+    // Check if the clear value is out of allowed range
+    if (temp_result > -1 && abs(int(best_color.c) - int(colorList[temp_result].c)) > C_ALLOWED_COLOR_VARIANCE) {
+      // If the clear value is out of range, then set this result back to unknown
+      temp_result = -1;
+    }
+
+    // If we've got a temp result
+    if (temp_result != -1) {
+      colorResults[skittleCount] = temp_result;
+    }
+
+    if (!HAS_RESULT(colorResults[skittleCount]) && retried < C_RETRY_UNKNOWN_COLOR ) {
+      // If color cannot be detected, try again
+      retried++;
+      isRemeasuring = true;
+      Serial.println("Color cannot be determined. Go back and re-measure it.");
+    }
+    if (isRemeasuring) {
+      remeasuringReverseDirection = !remeasuringReverseDirection;
+      Serial.print("The top servo is now going ");
+      Serial.println(remeasuringReverseDirection ? "backwards" : "forwards");
+      if (HAS_RESULT(colorResults[skittleCount])) {
+        // If we've got a result on re-measurement
+        Serial.println("We've got the color on re-measurement.");
+        isRemeasuring = false;
+        remeasuringReverseDirection = false;
+        retried = 0;
+      }
+      if (retried >= C_RETRY_UNKNOWN_COLOR) {
+        // If tried many times still can't get the color
+        // Just f*ck it cuz we ain't gonna get color however we try
+        Serial.println("re-measurement still couldn't get the color.");
+        isRemeasuring = false;
+        remeasuringReverseDirection = false;
+        retried = 0;
+      }
+    }
+#endif
+  }
+  // Serial.println("Finish measuring.");
+  isColorBeingMeasured = false;
+  lastSkittleTime = millis();
+  if (!isRemeasuring && HAS_RESULT(colorResults[skittleCount])) {
+    skittleCount++;
+  }
+}
+
 void updateColorSensor() {
-  // Is the color sensor measuring the Skittle right now
-  static boolean isColorBeingMeasured = false;
   // The best colors during a measuring cycle
   static uint16_t min_clear;
   static C_Color best_color = C_Color(0, 0 , 0);

    isColorBeingMeasured = true;
    min_clear = C_HOLE_CLEAR;
  }
  if (isColorBeingMeasured) {
    if (colors.c < min_clear && colors.c > C_IDEAL_CLEAR) {
      min_clear = colors.c;
      best_color = colors;
    }
    if (colors.c > C_HOLE_CLEAR) {
      best_color.maximize(); // Maximize color
      if (best_color.compare(C_COLOR_EMPTY).aggregate() < 16) {
        // If this is an empty hole.
        Serial.println("Empty hole");
      } else {
        skittleCount ++;
        Serial.println("A Skittle has been detected!");
        //Serial.println("Maximized colors:");
        //best_color.print();

        #if F_CALIBRATING 
          // If we are calibrating colors
          __calibrating(best_color);
        #else
          // Compare the reported color with the pre-defined colors in the colorList
          // and set the result to the color with the least color variance, if the 
          // variance is still under the allowed variance range (C_ALLOWED_COLOR_VARIANCE)
    
          int min_diff = C_ALLOWED_COLOR_VARIANCE; // Set the mininum difference to the allowed color variance
          sortingResult = -1; // Set the result to -1
           
          for (int i = 0; i < sizeof(colorList) / 8; i++) {
            // Compare the reported color with the color defined in the colorList
            C_Color diff = best_color.compare(colorList[i]);
            
            // Add the color difference's primary colors (R + G + B) together
            int agg = diff.aggregate();
            // Check if the aggregated color difference value is less than the minimum color difference
            if (agg < min_diff) {
              // If this is less than the minimun
              min_diff = agg; // Set the minimun difference to this aggregated color difference
              sortingResult = i; // Set the result to this color's index
            }            
          }
        #endif
      }
      // Serial.println("Finish measuring.");
      isColorBeingMeasured = false;
      lastSkittleTime = millis();
    }
  }


#define MIN_BTM_ANGLE 30 // Minimum bottom servo angle
#define NUM_OF_CUPS 5 // The number of cups at the bottom
#define EACH_CUP_ANGLE_INTERVAL ((MAX_BTM_ANGLE - MIN_BTM_ANGLE) / NUM_OF_CUPS)
  int result;
  if (skittleCount > 1 && HAS_RESULT(colorResults[skittleCount - 1])) {
    result = colorResults[skittleCount - 1];
  } else if (skittleCount > 2) {
    result = colorResults[skittleCount - 2];
  } else {
    result = 2;
  }
  int btmAngle = MIN_BTM_ANGLE + EACH_CUP_ANGLE_INTERVAL * result;
  servoBtm.write(btmAngle);
}

