CANDY_SORT

#include <avr/io.h>
#include <avr/delay.h>
#include <avr/interrupt.h>
#include "tcs3200.h"

#define CL_PURPLE     0x701
#define CL_YELLOW     0x702
#define CL_GREEN      0x703
#define CL_ORANGE     0x704
#define CL_RED        0x705

#define POS_COLLECT   0x801
#define POS_SENSOR    0x802
#define POS_DROP      0x803

#define SRV_STOP      0x555

#define ERR_NO_CANDY  0x999

#define CL_PURPLE_R   102
#define CL_PURPLE_G   81
#define CL_PURPLE_B   103
#define CL_PURPLE_C   305

#define CL_YELLOW_R   139
#define CL_YELLOW_G   110
#define CL_YELLOW_B   115
#define CL_YELLOW_C   384

#define CL_GREEN_R    108
#define CL_GREEN_G    97
#define CL_GREEN_B    106
#define CL_GREEN_C    331

#define CL_ORANGE_R   130
#define CL_ORANGE_G   91
#define CL_ORANGE_B   108
#define CL_ORANGE_C   348

#define CL_RED_R      113
#define CL_RED_G      82
#define CL_RED_B      103
#define CL_RED_C      317


volatile int measureReady;
volatile int overflowTicks;
volatile int actuallArmPosition;

void moveArm (int colorID) {
    DDRB |= (1 << PB1);     // PWM Output
    TCCR1A = 0;
    ICR1 = 19999;           // 20ms pulse width - 50Hz

    TCCR1A = (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);

    TCCR1A |= 2 << 6;       // PB1 as PWM Output

    actuallArmPosition = colorID;

    switch (colorID) {
        case CL_PURPLE:
            OCR1A = 600;        // PURPLE
            break;

        case CL_YELLOW:
            OCR1A = 1050;       // YELLOW
            break;

        case CL_GREEN:
            OCR1A = 1500;       // GREEN
            break;

        case CL_ORANGE:
            OCR1A = 1950;       // ORANGE
            break;

        case CL_RED:
            OCR1A = 2400;       // RED
            break;

        case SRV_STOP:          // SERVO STOP
            OCR1A = 0;
            break;
    }
}

void spinWheel (int moveTo) {
    DDRB |= (1 << PB2);
    TCCR1A = 0;
    ICR1 = 19999;

    TCCR1A = (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);

    TCCR1A |= 2 << 4;

    switch (moveTo) {
        case POS_COLLECT:       // POSITION 1
            OCR1B = 2350;
            break;

        case POS_SENSOR:        // POSITION 2
            OCR1B = 1420;
            break;

        case POS_DROP:          // POSITION 3
            OCR1B = 600;
            break;

        case SRV_STOP:          // SERVO STOP
            OCR1B = 0;
            break;
    }
}

void collectCandy() {
    spinWheel(POS_COLLECT);
    _delay_ms(1500);
    spinWheel(SRV_STOP);
}

uint32_t checkRed() {
    PORTC &= ~(1 << PC4);   // S2 - L
    PORTC &= ~(1 << PC5);   // S3 - L
    _delay_ms(100);
    return TCSMeasure();
}


uint32_t checkBlue() {
    PORTC &= ~(1 << PC4);   // S2 - L
    PORTC |= (1 << PC5);    // S3 - H
    _delay_ms(100);
    return TCSMeasure();
}

uint32_t checkGreen() {
    PORTC |= (1 << PC4);    // S2 - H
    PORTC |= (1 << PC5);    // S3 - H
    _delay_ms(100);
    return TCSMeasure();
}

uint32_t checkClear(){
    PORTC |= (1<<PC4);
    PORTC &= ~(1<<PC5);
    _delay_ms(100);
    return TCSMeasure();
}


int pickColor(uint32_t R, uint32_t G, uint32_t B, uint32_t C) {
    int CL_PURPLE_MISTAKE = 0;
    int CL_YELLOW_MISTAKE = 0;
    int CL_GREEN_MISTAKE = 0;
    int CL_ORANGE_MISTAKE = 0;
    int CL_RED_MISTAKE = 0;

//---PURPLE---
    if (CL_PURPLE_R > R) CL_PURPLE_MISTAKE += CL_PURPLE_R - R;
    else if (CL_PURPLE_R <= R) CL_PURPLE_MISTAKE += R - CL_PURPLE_R;

    if (CL_PURPLE_G > G) CL_PURPLE_MISTAKE += CL_PURPLE_G - G;
    else if (CL_PURPLE_G <= G) CL_PURPLE_MISTAKE += G - CL_PURPLE_G;

    if (CL_PURPLE_B > B) CL_PURPLE_MISTAKE += CL_PURPLE_B - B;
    else if (CL_PURPLE_B <= B) CL_PURPLE_MISTAKE += B - CL_PURPLE_B;

    if (CL_PURPLE_C > C) CL_PURPLE_MISTAKE += CL_PURPLE_C - C;
    else if (CL_PURPLE_C <= C) CL_PURPLE_MISTAKE += C - CL_PURPLE_C;


//---YELLOW
    if (CL_YELLOW_R > R) CL_YELLOW_MISTAKE += CL_YELLOW_R - R;
    else if (CL_YELLOW_R <= R) CL_YELLOW_MISTAKE += R - CL_YELLOW_R;

    if (CL_YELLOW_G > G) CL_YELLOW_MISTAKE += CL_YELLOW_G - G;
    else if (CL_YELLOW_G <= G) CL_YELLOW_MISTAKE += G - CL_YELLOW_G;

    if (CL_YELLOW_B > B) CL_YELLOW_MISTAKE += CL_YELLOW_B - B;
    else if (CL_YELLOW_B <= B) CL_YELLOW_MISTAKE += B - CL_YELLOW_B;

    if (CL_YELLOW_C > C) CL_YELLOW_MISTAKE += CL_YELLOW_C - C;
    else if (CL_YELLOW_C <= C) CL_YELLOW_MISTAKE += C - CL_YELLOW_C;


//---GREEN
    if (CL_GREEN_R > R) CL_GREEN_MISTAKE += CL_GREEN_R - R;
    else if (CL_GREEN_R <= R) CL_GREEN_MISTAKE += R - CL_GREEN_R;

    if (CL_GREEN_G > G) CL_GREEN_MISTAKE += CL_GREEN_G - G;
    else if (CL_GREEN_G <= G) CL_GREEN_MISTAKE += G - CL_GREEN_G;

    if (CL_GREEN_B > B) CL_GREEN_MISTAKE += CL_GREEN_B - B;
    else if (CL_GREEN_B <= B) CL_GREEN_MISTAKE += B - CL_GREEN_B;

    if (CL_GREEN_C > C) CL_GREEN_MISTAKE += CL_GREEN_C - C;
    else if (CL_GREEN_C <= C) CL_GREEN_MISTAKE += C - CL_GREEN_C;


//---ORANGE
    if (CL_ORANGE_R > R) CL_ORANGE_MISTAKE += CL_ORANGE_R - R;
    else if (CL_ORANGE_R <= R) CL_ORANGE_MISTAKE += R - CL_ORANGE_R;

    if (CL_ORANGE_G > G) CL_ORANGE_MISTAKE += CL_ORANGE_G - G;
    else if (CL_ORANGE_G <= G) CL_ORANGE_MISTAKE += G - CL_ORANGE_G;

    if (CL_ORANGE_B > B) CL_ORANGE_MISTAKE += CL_ORANGE_B - B;
    else if (CL_ORANGE_B <= B) CL_ORANGE_MISTAKE += B - CL_ORANGE_B;

    if (CL_ORANGE_C > C) CL_ORANGE_MISTAKE += CL_ORANGE_C - C;
    else if (CL_ORANGE_C <= C) CL_ORANGE_MISTAKE += C - CL_ORANGE_C;

//---RED
    if (CL_RED_R > R) CL_RED_MISTAKE += CL_RED_R - R;
    else if (CL_RED_R <= R) CL_RED_MISTAKE += R - CL_RED_R;

    if (CL_RED_G > G) CL_RED_MISTAKE += CL_RED_G - G;
    else if (CL_RED_G <= G) CL_RED_MISTAKE += G - CL_RED_G;

    if (CL_RED_B > B) CL_RED_MISTAKE += CL_RED_B - B;
    else if (CL_RED_B <= B) CL_RED_MISTAKE += B - CL_RED_B;

    if (CL_RED_C > C) CL_RED_MISTAKE += CL_RED_C - C;
    else if (CL_RED_C <= C) CL_RED_MISTAKE += C - CL_RED_C;


    int minimumMistake = 1000;
    int closestColor = 0;

    if (CL_PURPLE_MISTAKE < minimumMistake) {
        minimumMistake = CL_PURPLE_MISTAKE;
        closestColor = CL_PURPLE;
    }

    if (CL_YELLOW_MISTAKE < minimumMistake) {
        minimumMistake = CL_YELLOW_MISTAKE;
        closestColor = CL_YELLOW;
    }

    if (CL_GREEN_MISTAKE < minimumMistake) {
        minimumMistake = CL_GREEN_MISTAKE;
        closestColor = CL_GREEN;
    }

    if (CL_ORANGE_MISTAKE < minimumMistake) {
        minimumMistake = CL_ORANGE_MISTAKE;
        closestColor = CL_ORANGE;
    }

    if (CL_RED_MISTAKE < minimumMistake) {
        minimumMistake = CL_RED_MISTAKE;
        closestColor = CL_RED;
    }

    return closestColor;
}
void checkColor() {
    spinWheel(POS_SENSOR);
    _delay_ms(700);
    //spinWheel(SRV_STOP);
    //int nextPosition;
    uint32_t R=0;
    uint32_t G=0;
    uint32_t B=0;
    uint32_t C=0;

    int i;

    for (i = 0 ; i < 5 ; i++) R += checkRed()/100;
    for (i = 0 ; i < 5 ; i++) G += checkGreen()/100;
    for (i = 0 ; i < 5 ; i++) B += checkBlue()/100;
    for (i = 0 ; i < 5 ; i++) C += checkClear()/100;

    R = R/5;
    G = G/5;
    B = B/5;
    C = C/5;

    int nextPosition = pickColor(R, G, B, C);

    if (nextPosition != ERR_NO_CANDY) {
        moveArm(nextPosition);
        _delay_ms(1000);
        moveArm(SRV_STOP);
    }
}


void dropCandy() {
    spinWheel(POS_DROP);
    _delay_ms(700);
}

void moveServo(int pulseWidth) {
    TCCR1A = 0;
    ICR1 = 19999;

    TCCR1A = (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);

    TCCR1A |= 2 << 6;

    OCR1A = pulseWidth;
}

void MAIN_INIT() {
    DDRC |= (1 << PC5) | (1 << PC4) | (1 << PC3) | (1 << PC2) | (1 << PC1);
    PORTC |= (1 << PC3);

    moveArm(CL_GREEN);
    _delay_ms(1000);
    moveArm(SRV_STOP);

    spinWheel(POS_COLLECT);
    _delay_ms(1000);
    spinWheel(SRV_STOP);
}

void MAIN_LOOP() {
    while (1) {
        collectCandy();
        checkColor();
        dropCandy();
    }
}

int main() {

    moveArm(CL_RED);
    _delay_ms(1000);
    moveArm(SRV_STOP);

    MAIN_INIT();
    MAIN_LOOP();

    return 0;
}