/*
 * Skittles Color Sorter using Arduino UNO R3.
 * S0, S1, S2, S3 - PINs used by the color sensor.
 * Colours return IDs: 1 - red, 2 - green, 3 - yellow, 4 - orange, 5 - blue
 */

#include <Servo.h>

 /*
 * PIN-urile pentru senzorul de lumina.
 */
#define S0 3
#define S1 4
#define S2 5
#define S3 6

#define sensorOut 7

 /*
 * Coduri pentru culori.
 */
#define RED     1
#define GREEN   2
#define YELLOW  3
#define ORANGE  4
#define BLUE    5

#define START_POS   100 // 100°
#define SENSOR_POS  50  // 65°
#define END_POS     29  // 29°

#define DEBUG

Servo topServo;     // motor sus
Servo bottomServo;  // motor jos

int frequency = 0, color = 0;
int R, G, B;

void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);

  // Setting frequency-scaling to 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);

  topServo.attach(8);     // PIN #8
  bottomServo.attach(9);  // PIN #9

  Serial.begin(9600);
}

void loop() {
  topServo.write(START_POS); // seteaza servomotorul sus la 115° (ia bomboana)
  delay(500); // asteapta 500ms

  for (int i = START_POS; i >= SENSOR_POS; i--) { // muta motorul sub senzor
    topServo.write(i); // 110 - 66° cu delay de 2ms intre fiecare °
    delay(4); // 2ms
  }

  delay(500); // 500ms

  getRGB(&R, &G, &B); // afla culoarea
  color = getColor(R, G, B); // afla culoarea

  delay(10); // delay 10ms

  switch (color) {
    case RED: {
      //Serial.println("RED");
      bottomServo.write(50); // 50°
      break;
    }
    case GREEN: {
      Serial.println("GREEN");
      bottomServo.write(75); // 75°
      break;
    }
    case YELLOW: {
      Serial.println("YELLOW");
      bottomServo.write(100); // 100°
      break;
    }
    case ORANGE: {
      Serial.println("ORANGE");
      bottomServo.write(125); // 125°
      break;
    }
    case BLUE: {
      //Serial.println("BLUE");
      bottomServo.write(150); // 150°
      break;
    }

    case 0: break; // invalid colour
  }

  delay(300); // delay 300ms

  for (int i = SENSOR_POS; i >= END_POS; i--) {
    topServo.write(i);
    delay(4);
  }

  delay(200); // 200ms

  for (int i = END_POS; i <= START_POS; i++) {
    topServo.write(i); // revine la pozitia initiala de 115°
    delay(4);
  }

  color = 0; // reseteaza variabila de culoare
}

void getRGB(int *R, int *G, int *B)
{
  // Setting red filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);

  frequency = pulseIn(sensorOut, LOW);
  *R = frequency;
  // frequency = map(frequency, 165, 180, 255, 0);

  #if defined DEBUG
    Serial.print("R= ");//printing name
    Serial.print(frequency);//printing RED color frequency
    Serial.print("  ");
  #endif
  delay(100);

  // Setting Green filtered photodiodes to be read
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);

  frequency = pulseIn(sensorOut, LOW);
  *G = frequency;
  // frequency = map(frequency, 195, 210, 255, 0);

  #if defined DEBUG
    Serial.print("G= ");//printing name
    Serial.print(frequency);//printing RED color frequency
    Serial.print("  ");
  #endif

  delay(100);

  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);

  frequency = pulseIn(sensorOut, LOW);
  *B = frequency;
  // frequency = map(frequency, 30, 90, 255, 0);

  #if defined DEBUG
    Serial.print("B= ");//printing name
    Serial.print(frequency);//printing RED color frequency
    Serial.println("  ");
  #endif
  delay(100);
}

int getColor(int R, int G, int B)
{
  if ((R <= 105 && R >= 90) && (G <= 128 && G >= 110) && (B >= 90 && B <= 105))
    return RED; // RED

  else if ((R >= 85 && R <= 95) && (G >= 85 && G <= 100) && (B >= 83 && B <= 100))
    return GREEN; // GREEN

  else if ((R >= 70 && R <= 80) && (G >= 85 && G <= 90) && (B >= 79 && B <= 90))
    return YELLOW; // YELLOW

  else if ((R >= 70 && R <= 90) && (G >= 94 && G <= 114) && (B >= 82 && B <= 106))
    return ORANGE; // ORANGE

  //return 0; // INVALID
}