/*     Arduino Project - Color Sorting Machine
 *
 *  by Dejan Nedelkovski, www.HowToMechatronics.com
 *
 */
 //https://guilhermerodrigues680.github.io/wav2c-online/
 //https://andykong.org/blog/arduinodirect2wav/s
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

#define S0 2
#define S1 3
#define S2 4
#define S3 5
#define sensorOut 6

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

int R=0;
int G=0;
int B=0;
int Y=0;
int O=0;
int BR=0;

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

Servo topServo;
Servo bottomServo;

int frequency = 0;
int color = 0;
int prevcolor = 0;

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(7);
    bottomServo.attach(8);

    // LCD Init
     lcd.begin();
     lcd.backlight();

    lcd.setCursor(0, 0);
    lcd.print(" Cur Color: ");
    lcd.setCursor(0,1);
    lcd.print(" R:    G:    B:");
    lcd.setCursor(0,2);
    lcd.print(" Y:    O:    BR:");

    Serial.begin(9600);
}

void loop() {
    //Servo arm position 1. Pick skittle
    topServo.write(95);
    lcd.setCursor(1, 3);
    lcd.print("1. Load Skittle");
    delay(500); //wait half second

    //Servo arm position 2. Move servo arm under the sensor
    for (int i = 95; i > 40; i--) {
        topServo.write(i);
        delay(2);
    }
    lcd.setCursor(1, 3);
    lcd.print("2. Read Color  ");
    delay(500);

    //read color
    color = readColor(); 
    delay(10);
    // Comment below lines when writing to arduino
    //--------------------------------
    //color = random(1, 7);
    //while(prevcolor == color)
    //{
    //  color = random(1, 7);
   // } 
    //prevcolor = color;
    //--------------------------------
    Serial.print("Color Read: ");
    Serial.println(color);
    //Move lower servo based on coler
    switch (color) {
    case 1: //Red
        bottomServo.write(43);
        R++;
        lcd.setCursor(3,1);
        lcd.print(R);
        lcd.setCursor(12, 0);
        lcd.print("Red   ");
        break;

    case 2: //Orange
        bottomServo.write(71);
        O++;
        lcd.setCursor(9,2);
        lcd.print(O);
        lcd.setCursor(12, 0);
        lcd.print("Orange");
        break;

    case 3: //Green
        bottomServo.write(100);
        G++;
        lcd.setCursor(9,1);
        lcd.print(G);
        lcd.setCursor(12, 0);
        lcd.print("Green ");
        break;

    case 4: //Yellow
        bottomServo.write(127);
        Y++;
        lcd.setCursor(3,2);
        lcd.print(Y);
        lcd.setCursor(12, 0);
        lcd.print("Yellow");
        break;

    case 5: //Brown
        bottomServo.write(153);
        BR++;
        lcd.setCursor(16,2);
        lcd.print(BR);
        lcd.setCursor(12, 0);
        lcd.print("Brown ");
        break;

    case 6: //Blue
        bottomServo.write(178);
        B++;
        lcd.setCursor(15,1);
        lcd.print(B);
        lcd.setCursor(12, 0);
        lcd.print("Blue  ");
        break;

    case 0:
        break;
    }
    delay(300);

    //Move top servo arm to drop skittle
    for (int i = 40; i > 5; i--) {
        topServo.write(i);
        delay(2);
    }
    lcd.setCursor(1, 3);
    lcd.print("3. Drop Skittle");
    delay(500);

    //Move top servo arm to original position 
    for (int i = 5; i < 95; i++) {
        topServo.write(i);
        delay(2);
    }
    color = 0;
    delay(500);
}

// Custom Function - readColor()
int readColor() {
    // Setting red filtered photodiodes to be read
    digitalWrite(S2, LOW);
    digitalWrite(S3, LOW);
    // Reading the output frequency
    frequency = pulseIn(sensorOut, LOW);
    int R = frequency;
    // Printing the value on the serial monitor
    Serial.print("R= ");//printing name
    Serial.print(frequency);//printing RED color frequency
    Serial.print("  ");
    delay(50);

    // Setting Green filtered photodiodes to be read
    digitalWrite(S2, HIGH);
    digitalWrite(S3, HIGH);
    // Reading the output frequency
    frequency = pulseIn(sensorOut, LOW);
    int G = frequency;
    // Printing the value on the serial monitor
    Serial.print("G= ");//printing name
    Serial.print(frequency);//printing GREEN color frequency
    Serial.print("  ");
    delay(50);

    // Setting Blue filtered photodiodes to be read
    digitalWrite(S2, LOW);
    digitalWrite(S3, HIGH);
    // Reading the output frequency
    frequency = pulseIn(sensorOut, LOW);
    int B = frequency;
    // Printing the value on the serial monitor
    Serial.print("B= ");//printing name
    Serial.print(frequency);//printing BLUE color frequency
    Serial.println("  ");
    delay(50);

    if (R < 130 & R>115 & G < 165 & G>150) {
        color = 1; // Red
        Serial.println("Red Skittle");
    }
    if (R < 105 & R>90 & B < 150 & B>135) {
        color = 2; // Orange
        Serial.println("Orange Skittle");
    }
    if (R < 140 & R>125 & G < 125 & G>110) {
        color = 3; // Green
        Serial.println("Green Skittle");
    }
    if (R < 95 & R>80 & G < 105 & G>90) {
        color = 4; // Yellow
        Serial.println("Yellow Skittle");
    }
    if (R < 170 & R>150 & G < 175 & G>160) {
        color = 5; // Brown
        Serial.println("Brown Skittle");
    }
    if (G < 155 & G>140 & B < 130 & B>115) {
        color = 6; // Blue
        Serial.println("Blue Skittle");
    }
    return color;
}