/*
  https://circuitdigest.com/microcontroller-projects/arduino-color-sorter-machine-using-tcs3200-color-sensor
  https://www.youtube.com/watch?v=1qr6v4YTz38
  https://howtomechatronics.com/projects/arduino-color-sorter-project/

  +----------------------------------------+
  | +------------------------------------+ |
  | |  ===========                       | |
  | | | ARDUINO  |            =======    | |
  | | |          |     SERVO |DROPsrv|   | |
  | | |       D10|->---------|PWM GND|---+ |
  | | |          |           |    +5V|---|-+
  | | |          |            =======    | |
  | +-| 5V       |     SERVO |PICKsrv|   | |
  +---|GND    D09|->---------|PWM GND|---+ |
      |          |           |    +5V|---|-+
      |          |            =======    | |
      |          |         +-------------|-+
      |          |         |  =========  |
      |          |         | | TSC3200 | |
      |          |         +-|VCC   GND|-+
      |       D08|-----------|OUT    OE|-+
      |       D07|->---------|S2     S1|---+
      |       D06|->---------|S3     S0|-+ |
      |          |            =========  | |
      |       D05|->---------------------|-+
      |       D04|->---------------------+
      ============

      TSC3200
      S0 S1 OUTPUT FREQUENCY SCALING
      L  L  Power down
      L  H  2%
      H  L  20%
      H  H  100% 

      S2 S3 PHOTODIODE
      L  L  RED
      L  H  BLU
      H  L  CLR (no filter)
      H  H  GRN
*/

#include <Servo.h>
Servo dropServo;
Servo colorServo;

#define PICKsrv 9  // servo
#define DROPsrv 10

#define S0 4 // TSC3200
#define S1 5
#define S2 7
#define S3 6
#define sensorOut 8

int frequency = 0;
int color = 0;

void setup() {
  Serial.begin(9600);
  randomSeed(analogRead(A0)); // for simulation
  dropServo.attach(PICKsrv);
  colorServo.attach(DROPsrv);

  pinMode(S0, OUTPUT); // output to TSC3200
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);

  pinMode(PICKsrv, OUTPUT); // output to servos
  pinMode(DROPsrv, OUTPUT);
  
  pinMode(sensorOut, INPUT);

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

void loop() {
  // pick home/reload
  Serial.print("PICK(home/load) ");
  dropServo.write(115);
  delay(200); // let servo settle

  // drop home
  Serial.print("COLOR(home). ");
  colorServo.write(115);
  delay(200); // let servo settle

  //read color values by calling function. save the values for conclusion in variable
  color = detectColor();
  // delay(500); // allow for detect
  Serial.print(". Move ramp to ");
  switch (color) {
    case 1: // RED
      colorServo.write(50);
      Serial.print("RED");
      break;

    case 2: // ORG
      colorServo.write(80);
      Serial.print("ORG");
      break;

    case 3: // GRN
      colorServo.write(110);
      Serial.print("GRN");
      break;

    case 4: // YEL
      colorServo.write(140);
      Serial.print("YEL");
      break;

    case 5: // BLU
      colorServo.write(170);
      Serial.print("BLU");
      break;

    case 0:
      colorServo.write(0);
      Serial.print("NON");
      break;
  }

  // move lego to drop ramp
  Serial.println(". DROP (on ramp).");
  for (int i = 65; i > 29; i--) {
    dropServo.write(i);
  }
  delay(500); // remove comment
  color = 0;
}

int detectColor() {
  // activating RED photodiodes to read
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  frequency = pulseIn(sensorOut, LOW);
  frequency = random(20, 38); // RED limits for a random test value
  int R = frequency;
  Serial.print("RED");
  Serial.print(frequency);  // RED color frequency
  Serial.print(" ");

  // activating BLU photodiodes to read
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  frequency = pulseIn(sensorOut, LOW);
  frequency = random(19, 22); // BLUE limits for a random test value
  int B = frequency;
  Serial.print("BLU");
  Serial.print(frequency);
  Serial.print(" ");

  // activating GRN photodiodes to read
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  frequency = pulseIn(sensorOut, LOW);
  frequency = random(22, 44); // GREEN limits for a random test value
  int G = frequency;
  Serial.print("GRN");
  Serial.print(frequency);

  // Color readings are different for different setups. Change
  // the readings according your project and readings detected

  int xyz = random(1, 5);  // random color for the sim

  if (xyz == 1) // RED
  {
    R = 23;
    G = 29;
    B = 00;
  }
  if (xyz == 2) // ORG
  {
    R = 00;
    G = 22;
    B = 20;
  }
  if (xyz == 3) // GRN
  {
    R = 20;
    G = 44;
    B = 00;
  }
  if (xyz == 4) // YEL
  {
    R = 25;
    G = 30;
    B = 00;
  }
  if (xyz == 5) // BLU
  {
    R = 00;
    G = 27;
    B = 20;
  }

  Serial.print(". Detected ");

  if (R > 19 & R < 24 & G > 26 & G < 30) {
    color = 1;  // Red
    Serial.print("RED");
  }

  if (G > 21 & G < 26 & B > 18 & B < 23) {
    color = 2;  // Orange
    Serial.print("ORG");
  }

  if (R > 19 & R < 21 & G > 24 & G < 28) {
    color = 3;  // Green
    Serial.print("GRN");
  }

  if (R > 23 & R < 39 & G > 29 & G < 45) {
    color = 4;  // Yellow
    Serial.print( "YEL");
  }

  if (G > 26 & G < 30 & B > 18 & B < 23) {
    color = 5;  // Blue
    Serial.print("BLU");
  }
  if (color < 1 | color > 5)
    Serial.print("NON");

  return color;
}