 #include "Adafruit_TCS34725.h"  // Color Sensor
 
 // Initialize the display with the numbers of the interface pins
-LiquidCrystal lcd(pinLcdRS, pinLcdEnable, pinLcdD4, pinLcdD5, pinLcdD6, pinLcdD7);
+LiquidCrystal lcd(PIN_LCD_RS, PIN_LCD_ENABLE, PIN_LCD_D4, PIN_LCD_D5, PIN_LCD_D6, PIN_LCD_D7);
 // Create the object for the top continuous rotation servo
 Servo servoTop;
 // Create the object for the bottom standard servo
 Servo servoBtm;
 // Create the color sensor object
-Adafruit_TCS34725 tcs = Adafruit_TCS34725();
-
-// Variables
-int skittleCount = 0; // The number of skittles sorted
-unsigned long lastSkittleTime = 0; // Time when the last skittle was being sorted
-int topServoSpeed = 0; // The speed in which the top servo moves
-boolean isColorBeingMeasured = false; // Is the color sensor measuring the Skittle right now
-
+Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_1X);
+
+// The number of skittles sorted
+int skittleCount = 0;
+// Time when the last skittle was being sorted
+unsigned long lastSkittleTime = 0;
+// The speed in which the top servo moves
+int topServoSpeed = 0;
+// Is the color sensor measuring the Skittle right now
+boolean isColorBeingMeasured = false;
 
 void setup() {
   // Set up the serial for debugging purposes

//    Color View Blue pin to diginal pin 6
// * LCD
//    RS pin to digital pin 12
const int pinLcdRS = 12;
//    Enable pin to digital pin 11
const int pinLcdEnable = 11;
//    LCD D4 pin to digital pin 5
const int pinLcdD4 = 5;
//    LCD D5 pin to digital pin 4
const int pinLcdD5 = 4;
//    LCD D6 pin to digital pin 3
const int pinLcdD6 = 3;
//    LCD D7 pin to digital pin 2
const int pinLcdD7 = 2;
//    LCD R/W pin to ground


// Constants
const boolean topServoTimeoutEnable = true; // Enable the timeout for the top servo
const int topServoTimeout = 6000; // Timeout to determine if the top servo is stuck
const int rewindingTime = 500; // Time in milliseconds when the top servo rewinds to solve jam

// Include libraries
#include <LiquidCrystal.h>      // LCD display

  lcd.begin(16, 2);

  // Attach the servos on corresponding pins to the servo objects
  servoTop.attach(PIN_TOP_SERVO);
  servoBtm.attach(PIN_BTM_SERVO);

  if (!tcs.begin()) {
    // If the color sensor failed to initialize, print out an error
    lcd.print("ERR: Color Sensor Connection");
    while (1);
  }

  lcd.print("Color Sorter");

}

void updateTopServo() {
  static boolean reverseDirection = false;
  // Set the speed of the top servo
  // with 0 being full-speed in one direction, 180 being full speed in the other, and a value near 90 being no movement.
  servoTop.write(reverseDirection ? 180 : topServoSpeed);
  // servoTop.write(180);

  // If color is being measured right now, skip the stuck detection
  if (isColorBeingMeasured)
    return;

  // If the top servo gets stuck
  if (topServoTimeoutEnable && millis() - lastSkittleTime > topServoTimeout) {
    Serial.println("Top servo is stuck; Direction reversed.");
    // Reserve the top servo's direction
    reverseDirection = true;

    lastSkittleTime = millis();
  }

  if (topServoTimeoutEnable && reverseDirection && millis() - lastSkittleTime > 500) {
    // Set direction back to normal after .5s
    reverseDirection = false;
  }

}

void updateBtmServo() {

  // Calculate the angle of the bottom servo arm
  static int btmAngle = 0;
  static bool btmReversed = false;
  btmAngle += btmReversed ? -10 : 10;

  // The bottom arm angle range from 30 to 150 degrees
  if (btmAngle >= 150)
    btmReversed = true;
  if (btmAngle <= 30)
    btmReversed = false;

  // Set the angle of the shaft (in degrees), ranging from 0 to 180
  //servoBtm.write(90);
  servoBtm.write(btmAngle);
}

void updateColorSensor() {
  // Get color measurement values
  uint16_t r, g, b, c;
  tcs.getRawData(&r, &g, &b, &c);
  r /= C_CYCLES; g /= C_CYCLES; b /= C_CYCLES; c /= C_CYCLES;

  // If clear value is less than 300
  if (c < 150 && !isColorBeingMeasured) {
    // This is when the color sensor sees into the hole

    if (millis() - lastSkittleTime > 1000) {
      skittleCount ++;
      Serial.println(millis() - lastSkittleTime);
      lastSkittleTime = millis();

      // Slow down the top servo's speed for better accuracy
    /*
        topServoSpeed = 90;
        Serial.println("Start measuring this Skittle's color.");
      isColorBeingMeasured = true;
     */

    }

  }

  if (c < 150) {
    Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
    Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
    Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
    Serial.print("C: "); Serial.print(c, DEC); Serial.print(" ");
    Serial.println(" ");

    if (millis() - lastSkittleTime > 1000) {
      topServoSpeed = 0;
      Serial.println("Finish measuring.");
      isColorBeingMeasured = false;
      lastSkittleTime = millis();

    }
  }

    if (c < 300) {
      
    if (millis() - lastSkittleTime > 1000) 
      lastSkittleTime = millis();
    
    }

  /*
//    Serial.print("Color Temp: "); Serial.print(colorTemp, DEC); Serial.print(" K - ");
  //  Serial.print("Lux: "); Serial.print(lux, DEC); Serial.print(" - ");
    Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
    Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
    Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
    Serial.print("C: "); Serial.print(c, DEC); Serial.print(" ");
    Serial.println(" ");
  
*/


}

void loop() {
  // static byte frames = 0;

  lcd.clear();
  lcd.print("Count: "); lcd.print(skittleCount, DEC);

  // Update the top servo
  updateTopServo();

  // Update the bottom servo (arm)
  //updateBtmServo();

  // Update the color sensor
  updateColorSensor();

  delay(100);
}
