/*     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.init();
     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
    topServo.write(105);
    lcd.setCursor(1, 3);
    lcd.print("1. Load Stittle");
    delay(500); //wait half second

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

    //read color
    //color = readColor(); //uncomment this line when writing to arduino
    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(35);
        R++;
        lcd.setCursor(3,1);
        lcd.print(R);
        lcd.setCursor(12, 0);
        lcd.print("Red   ");
        break;

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

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

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

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

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

    case 0:
        break;
    }
    delay(300);

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

    for (int i = 29; i < 115; i++) {
        topServo.write(i);
        delay(2);
    }
    color = 0;
}

// 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 < 145 & R>130 & G < 170 & G>155) {
        color = 1; // Red
        Serial.println("Red Skittle");
    }
    if (R < 120 & R>105 & B < 150 & B>135) {
        color = 2; // Orange
        Serial.println("Orange Skittle");
    }
    if (R < 145 & R>135 & G < 135 & G>115) {
        color = 3; // Green
        Serial.println("Green Skittle");
    }
    if (R < 111 & R>95 & G < 125 & G>110) {
        color = 4; // Yellow
        Serial.println("Yellow Skittle");
    }
    if (R < 185 & R>170 & G < 185 & G>170) {
        color = 5; // Brown
        Serial.println("Brown Skittle");
    }
    if (G < 155 & G>130 & B < 125 & B>110) {
        color = 6; // Blue
        Serial.println("Blue Skittle");
    }
    return color;
}