////////////VARIAVEIS////////////
#define hivel_0 80
#define hivel_1 80
#define lovel_0 40
#define lovel_1 40
#define tempo_procura 900L

#define threshold 100
/////////////////////////////////

//////////RECONHECE COR//////////
/* Configuracao fisica dos pinos SPI

+5V    |X|X|  GND
bit 2  |X|X|  bit 5
bit 3  |X|X|  bit 4
N.C.   |X|X|  N.C.

+5V    |X|X|  GND
AZUL   |X|X|  BRANCO
ROXO   |X|X|  CINZA 
N.C.   |X|X|  N.C.

bits referentes ao endereco 0x1008

*/

void liga_led(int n) {
    int mask1, mask2;
    switch(n) {
        //VERMELHO
        case 0: {
            mask1 = 0x04;
            bit_clear(0x1008, 0x08);
            bit_clear(0x1008, 0x10);
            bit_clear(0x1008, 0x20);
        }break;
        //AZUL
        case 1: {
            mask1 = 0x10;
            bit_clear(0x1008, 0x04);
            bit_clear(0x1008, 0x08);
            bit_clear(0x1008, 0x20);
        }break;
        //VERDES
        case 2: {
            mask1 = 0x20;
            mask2 = 0x08;
            bit_clear(0x1008, 0x04);
            bit_clear(0x1008, 0x10);
            bit_set(0x1008, mask2);
        }break;
        default:
          return;
    }
    bit_set(0x1008, mask1);
}

void desliga_led() {
    bit_clear(0x1008, 0x04);
    bit_clear(0x1008, 0x08);
    bit_clear(0x1008, 0x10);
    bit_clear(0x1008, 0x20);
}

void reconhece_cor(int vermelho, int azul, int verde) {
    //PRETO: TODOS > 45
    if (vermelho > 45 && azul > 45 && verde > 45) {
        printf("PRETO\n");
        return;
    }
    //MARROM: MENOR VERMELHO MAS MAIOR QUE 25
    if (vermelho > 25 && azul > vermelho && verde > vermelho) {
        printf("MARROM\n");
        return;
    }
    //VERMELHO: MENOR VERMELHO  < 25
    else if (vermelho < 25 && azul > vermelho && verde > vermelho && verde > 25) {
        printf("VERMELHO\n");
        return;
    }
    //AMARELO: VERMELHO MENOR E VERDE MENOR QUE 25
    else if (azul > vermelho && verde > vermelho && verde < 25) {
        printf("AMARELO\n");
        return;
    }
    //VERDE: MENOR VERDE
    else if (vermelho > verde && azul > verde) {
        printf("VERDE\n");
        return;
    }
    //AZUL: MENOR AZUL
    else if (vermelho > azul && verde > azul) {
        printf("AZUL\n");
        return;
    }
    else {
        printf("NAO RECONHECI\n");
        return;
    }
    
}


void reconhecimento_cor() {
    int i = 0;
    int cores[3];
    
    // Converte os pinos SPI em saidas
    poke(0x1009, 0x3c);
    
    beep();
    printf("Reconhecimento  de Cores: START\n");
    while(!start_button());
    start_press();
    while(1) {
        if (start_button()){
            for(i = 0; i < 3; i++) {
                liga_led(i);
                sleep(0.15);
                cores[i] = analog(6);
                sleep(0.15);
                desliga_led();
            }
            reconhece_cor(cores[0], cores[1], cores[2]);
        }
        
    }
}



/////////////////////////////////

///////////SEGUE LINHA///////////
void segue_linha() {
    int s2; // Direita
    int s3; // Esquerda
    int ultima_linha = 1; // Direita = 1, Esquerda = -1
    
    beep();
    printf("Segue Linha     START\n");
    while(!start_button());
    start_press();
    printf("Procurando a    linha preta.\n");
    
    while(!stop_button()) {
        s2 = analog(2);
        s3 = analog(3);
        // Totalmente fora da linha
        if (s2 < threshold && s3 < threshold) {
            printf("Procurando Curva\n");
            // Procura na ultima linha
            motor(0, -ultima_linha*hivel_0);
            motor(1, ultima_linha*hivel_1);
            msleep(tempo_procura);
            motor(0, 0);
            motor(1, 0);
            s2 = analog(2);
            s3 = analog(3);
            if (s2 >= threshold || s3 >= threshold) {
                // Achou a curva
                printf("Achei a curva\n");
            }
            else {
                // Procura no sentido contrario
                motor(0, ultima_linha*hivel_0);
                motor(1, -ultima_linha*hivel_1);
                msleep(2L*tempo_procura);
                motor(0, 0);
                motor(1, 0);
                s2 = analog(2);
                s3 = analog(3);
                if (s2 >= threshold || s3 >= threshold) {
                    // Achou a curva
                    printf("Achei a curva\n");
                    ultima_linha = -ultima_linha;
                }
                else {
                    printf("Estou Perdido!\n");
                    while(!start_button() && !stop_button());
                }
            }
            
        }
        // Desviou para a esquerda
        else if (s2 < threshold && s3 >= threshold) {
            printf("Corrigindo...\n");
            ultima_linha = -1;
            motor(0, hivel_0);
            motor(1, lovel_1);
        }
        // Desviou para a direita
        else if (s2 >= threshold && s3 < threshold) {
            printf("Corrigindo...\n");
            ultima_linha = 1;
            motor(0, lovel_0);
            motor(1, hivel_1);
        }
        // Continua na linha
        else {
            printf("Seguindo a linha\n");
            motor(0, hivel_0);
            motor(1, hivel_1);
        }
    }   
}
/////////////////////////////////

int main() {
    int modo = -1;
    
    printf ("T-800        TP1  Aperte START \n");
    start_press();
    
    while(1) {
        if (knob() > 128) {
            if (modo != 0) {
                printf("Segue Linha\n");
            }
            modo = 0;
            if (start_button()) {
                segue_linha();
                modo = -1;
            }
        }
        else {
            if (modo != 1) {
                printf("Reconhece Cor\n");
            }
            modo = 1;
            if (start_button()) {
                reconhecimento_cor();
                modo = -1;
            }
        }
    }
}