//Sensores analogicos
#define SENSOR_VERM 2
#define SENSOR_VERD 3
#define SENSOR_AZUL 4
#define SENSOR_LUZ_DIR 5   //RP
#define SENSOR_LUZ_ESQ 6   //LP

//Sensores digitais
#define SENSOR_LINHA_DIR 8
#define SENSOR_LINHA_ESQ 7
#define SENSOR_LINHA_AUX 9
#define BREAKB_ESQ 10
#define BREAKB_DIR 11
#define SENSOR_BLOCO 14

//Motores
#define MOTOR_ESQ 3
#define MOTOR_DIR 1

//Estados de movimento
#define PARADO 0
#define RETO 1
#define VIRANDOESQ 2
#define VIRANDODIR 3
#define RE 4
#define LENDO 5
#define STANDBY 6

#define DIREITA90 0
#define ESQUERDA90 1
#define RETOXSEG 2
#define PARA 3
#define LEVAR_BASE 4
#define CUSPIR 5

//variaveis de sensores
int pulsosEsq=0, pulsosDir=0, luzEsq=0, luzDir=0, verm=0, verd=0, azul=0, e=0, d=0, aux=0;

//variaveis de controle
int movimento=0, xSeg=0, base=-1, azulOPT=LEVAR_BASE, amarOPT=LEVAR_BASE, verdOPT=LEVAR_BASE, vermOPT=LEVAR_BASE;
float contador=0.0;

void contaTempo(){
    contador=0.0;
    while(contador < 60.0){
        sleep(0.1);
        contador=contador+0.1;
    }
    para();
}

///// ///// ///// ///// FUNCOES BASICAS ///// ///// ///// ///// MOTOR ESQUERDA MAIS FRACO
void reto(){
    motor(MOTOR_ESQ, -80);
    motor(MOTOR_DIR, 80);
}

void retoEndireitando(){
    motor(MOTOR_DIR, 30);
}

void retoEsquerdando(){
    motor(MOTOR_ESQ, -30);
}

void re(){
    motor(MOTOR_ESQ, 65);
    motor(MOTOR_DIR, -65);
}

void para(){
    motor(MOTOR_ESQ, 0);
    motor(MOTOR_DIR, 0);
}

void giraEsq(){
    motor(MOTOR_ESQ, 70);
    motor(MOTOR_DIR, 65);
}

void giraDir(){
    motor(MOTOR_ESQ, -70);
    motor(MOTOR_DIR, -65);
}

void contornaEsq(){
    motor(MOTOR_ESQ, -70);
    motor(MOTOR_DIR, 0);
}

void contornaDir(){
    motor(MOTOR_ESQ, 0);
    motor(MOTOR_DIR, 65);
}

///// ///// ///// ///// FUNCOES PARA VERIFICAR SENSORES ///// ///// ///// /////
int saiuEsquerda(){
    //e=analog(SENSOR_LINHA_ESQ);
    return e;
}

int saiuDireita(){
    //d=analog(SENSOR_LINHA_DIR);
    return d;
}

void leSensoresCor(){
    verm=analog(SENSOR_VERM);
    verd=analog(SENSOR_VERD);
    azul=analog(SENSOR_AZUL);
}

void leSensoresLinha(){
    while(1){
        leLinha();
    }
}

void leLinha(){
    e=digital(SENSOR_LINHA_ESQ);
    d=digital(SENSOR_LINHA_DIR);
    aux=digital(SENSOR_LINHA_AUX);
}


void contaPulsosBBeams(){
    int estado=0, anterior=0;                     //EsqDir=X ->  00=0, 01=1, 10=2, 11=3;
    
    estado=digital(BREAKB_DIR);
    estado+=digital(BREAKB_ESQ)*2;
    pulsosEsq=0;
    pulsosDir=0;
    
    while(1){
        estado=digital(BREAKB_DIR);
        estado+=digital(BREAKB_ESQ)*2;
        
        if(estado!=anterior){
            switch(anterior){
                case 0:{
                    switch(estado){
                        case 1: pulsosDir++; break;
                        case 2: pulsosEsq++; break;
                        case 3: pulsosDir++; pulsosEsq++; break;
                    }
                    break;
                }
                case 1:{
                    switch(estado){
                        case 0: pulsosDir++; break;
                        case 2: pulsosDir++; pulsosEsq++; break;
                        case 3: pulsosEsq++; break;
                    }
                    break;
                }
                case 2:{
                    switch(estado){
                        case 0: pulsosEsq++; break;
                        case 1: pulsosDir++; pulsosEsq++; break;
                        case 3: pulsosDir++; break;
                    }
                    break;
                }
                case 3:{
                    switch(estado){
                        case 0: pulsosDir++; pulsosEsq++; break;
                        case 1: pulsosEsq++; break;
                        case 2: pulsosDir++; break;
                    }
                    break;
                }
                
            }
        }
        anterior=estado;
    }
}

///// ///// ///// ///// FUNCOES DE MOVIMENTO ///// ///// ///// /////
void gira90Esq(){
    giraEsq();
    movimento=VIRANDOESQ;
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<9 || pulsosDir<9);
    para();
    movimento=PARADO;
}

void gira90Dir(){
    giraDir();
    movimento=VIRANDODIR;
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<9 || pulsosDir<9);
    para();
    movimento=PARADO;
}

void contorna90Dir(){
    contornaDir();
    movimento=VIRANDODIR;
    pulsosDir=0;
    while(pulsosDir<9);
    para();
    movimento=PARADO;    
}

void contorna90Esq(){
    contornaEsq();
    movimento=VIRANDOESQ;
    pulsosEsq=0;
    while(pulsosEsq<9);
    para();
    movimento=PARADO;    
}

void giraPoucoDir(){
    giraDir();
    movimento=VIRANDODIR;
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<3 || pulsosDir<3);
    para();
    movimento=PARADO;
}

void giraPoucoEsq(){
    giraEsq();
    movimento=VIRANDOESQ;
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<3 || pulsosDir<3);
    para();
    movimento=PARADO;
}

void ajustaPraDireita(){
    contornaEsq();
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<4  && d!=1);
    para();
    
    contornaDir();
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosDir<2 && d!=1);
    para();
}

void ajustaPraEsquerda(){
    contornaDir();
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosDir<4 && e!=1);
    para();
    
    contornaEsq();
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<2 && e!=1);
    para();
}

void pequenaRe(){
    re();
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<3 || pulsosDir<3);
    para();
}

void poucoReto(){
    reto();
    pulsosEsq=0;
    pulsosDir=0;
    while(pulsosEsq<3 || pulsosDir<3);
    para();
}

void calibraLuz(){
    int i=0, esqAtual=255, dirAtual=255, id=-1; 
    luzEsq=255;
    luzDir=255;
    for(i=0; i<4; i++){
        giraDir();
        movimento=VIRANDODIR;
        pulsosEsq=0;
        pulsosDir=0;
        
        while(pulsosEsq<9 || pulsosDir<9){
            esqAtual=analog(SENSOR_LUZ_ESQ);
            dirAtual=analog(SENSOR_LUZ_DIR);
            if(esqAtual<luzEsq) luzEsq = esqAtual;
            if(dirAtual<luzDir) luzDir = dirAtual;
            printf("luzEsq=%d luzDir=%d\n", luzEsq, luzDir);
            sleep(0.05);
        }
        para();
        movimento=PARADO;
    }
}

///// ///// ///// ///// ALGORITMOS ///// ///// ///// /////
void stopAtLP(){
    int esqAtual;
    
    esqAtual=analog(SENSOR_LUZ_ESQ);
    while(((float)esqAtual*0.9) > (float)luzEsq && contador<60.0){
        printf("luzESQ=%d\n", esqAtual);
        esqAtual=analog(SENSOR_LUZ_ESQ);
    }
    
    para();
}

void stopAtRP(){
    int dirAtual;
    
    dirAtual=analog(SENSOR_LUZ_DIR);
    while(((float)dirAtual*0.9) > (float)luzDir  && contador<60.0){
        printf("luzDIR=%d\n", dirAtual);
        dirAtual=analog(SENSOR_LUZ_DIR);
    }
    
    para();
}


void awayStopAtLP(){
    int i=0, esqAtual, dirAtual, luzE=255, luzD=255;
    
    for(i=0; i<4; i++){
        contornaEsq();
        movimento=VIRANDOESQ;
        pulsosDir=0;
        pulsosEsq=0;
        
        while(pulsosEsq<9 && pulsosDir<9 && contador<60.0){
            esqAtual=analog(SENSOR_LUZ_ESQ);
            dirAtual=analog(SENSOR_LUZ_DIR);
            if(esqAtual<luzE) luzE = esqAtual;
            if(dirAtual<luzD) luzD = dirAtual;
            printf("luzEsq=%d luzDir=%d\n", luzE, luzD);
            sleep(0.05);
        }
    }
    
    esqAtual=analog(SENSOR_LUZ_ESQ);
    while(((float)esqAtual*0.9) > (float)luzE && contador<60.0){
        printf("luzESQ=%d\n", esqAtual);
        esqAtual=analog(SENSOR_LUZ_ESQ);
    }
    
    para();
}

void awayStopAtRP(){
    int i=0, esqAtual, dirAtual, luzE=255, luzD=255;
    
    
    for(i=0; i<4; i++){
        contornaEsq();
        movimento=VIRANDOESQ;
        pulsosEsq=0;
        pulsosDir=0;
        
        while(pulsosEsq<9 && pulsosDir<9 && contador<60.0){
            esqAtual=analog(SENSOR_LUZ_ESQ);
            dirAtual=analog(SENSOR_LUZ_DIR);
            if(esqAtual<luzE) luzE = esqAtual;
            if(dirAtual<luzD) luzD = dirAtual;
            printf("luzEsq=%d luzDir=%d\n", luzE, luzD);
            sleep(0.05);
        }
    }
    
    dirAtual=analog(SENSOR_LUZ_DIR);
    while(((float)dirAtual*0.9) > (float)luzD && contador<60.0){
        printf("luzDIR=%d\n", dirAtual);
        dirAtual=analog(SENSOR_LUZ_DIR);
    }
    
    para();
}


void afastaLuzLP(){
    int esqAtual=255;
    giraDir();
    stopAtLP();
    
    gira90Dir();
    gira90Dir();
    para();
    movimento=RETO;
    reto();
}

void afastaLuzRP(){
    int dirAtual=255;
    giraDir();
    stopAtRP();
    
    gira90Dir();
    gira90Dir();
    para();
    movimento=RETO;
    reto();
}

void segueReto(){
    reto();
    while(movimento==RETO && contador < 60.0){
        if(pulsosEsq < pulsosDir)      retoEndireitando();
        else if(pulsosEsq > pulsosDir) retoEsquerdando();
        else                           reto();
    }
}

void retoAteLinha(){
    movimento=RETO;
    reto();
    leLinha();
    while(movimento==RETO && e==1 && d==1 && contador < 60.0){
        if(pulsosEsq < pulsosDir)      retoEndireitando();
        else if(pulsosEsq > pulsosDir) retoEsquerdando();
        else                           reto();
        leLinha();
    }
    para();
    movimento=PARADO;
}

void andaLinhaEscura(){
    int viradas=0;
    while(start_button());
    
    while(!stop_button() && contador<60.0){
        if(saiuEsquerda()==0 && saiuDireita()==0){   
            printf("To na linha!!\n");
            reto();
        }
        else if(saiuEsquerda()==1 && saiuDireita()==0){   
            printf("Ajustando pra direita\n");
            ajustaPraDireita();
        }
        else if(saiuEsquerda()==0 && saiuDireita()==1){
            printf("Ajustando pra esquerda\n");
            ajustaPraEsquerda();
        }
        else{
            printf("Sai com 2 sensores!");  //ESTRELA
            if(aux==1){
                while(saiuEsquerda()==1 && saiuDireita()==1 && viradas<4 && contador<60.0){
                    giraPoucoDir();
                    poucoReto();
                    if(saiuEsquerda()==1 && saiuDireita()==1) pequenaRe();
                    else giraPoucoEsq();
                    viradas++;
                }
                while(saiuEsquerda()==1 && saiuDireita()==1 && viradas>=4 && contador<60.0){
                    giraPoucoEsq();
                    poucoReto();
                    if(saiuEsquerda()==1 && saiuDireita()==1) pequenaRe();
                    else giraPoucoDir();
                }
            }else{
                while(saiuEsquerda()==1 && saiuDireita()==1 && viradas<4 && contador<60.0){
                    giraPoucoEsq();
                    poucoReto();
                    if(saiuEsquerda()==1 && saiuDireita()==1) pequenaRe();
                    else giraPoucoDir();
                    viradas++;
                }
                while(saiuEsquerda()==1 && saiuDireita()==1 && viradas>=4 && contador<60.0){
                    giraPoucoDir();
                    poucoReto();
                    if(saiuEsquerda()==1 && saiuDireita()==1) pequenaRe();
                    else giraPoucoEsq();
                }
            }
            viradas=0;
        }
    }
    
    para();
    while(stop_button());
}   

void obedeceBloco(){
    int opc=0, todo=-1, pid=0, pid2=0;
    float garantia=0.0;
    
    pid2=start_process(leSensoresLinha());
    pid=start_process(andaLinhaEscura());
    
    while(!stop_button() && contador<60.0){
        if(digital(SENSOR_BLOCO)==1){
            //le o mesmo valor por um mesmo periodo de tempo
            
            leSensoresCor();
            if(pid!=0){ kill_process(pid); pid=0; }
            
            //para(); //motivo para comentar esse codigo: continuar carregando o bloco para a base
            movimento=LENDO;
            
            if(     verm<130  && azul<120){
                //printf("amarelo         ");
                printf("coletar         ");
                todo=amarOPT;
            }
            else if(verm<165  && azul>120){
                //printf("vermelho        ");
                printf("coletar         ");
                todo=vermOPT;
            }
            else if(verm>165  && verd<220){
                //printf("verde           ");
                printf("coletar         ");
                todo=verdOPT;
            }
            else if(verm>165  && verd>220){
                //printf("azul            ");
                printf("coletar         ");
                todo=azulOPT;
            }
            else{
                printf("desconhecido ...");
                todo=-1;
            }
            
            sleep(0.1);
            garantia+=0.1;
        }
        
        if(garantia > 1.0){
            switch(todo){
                case LEVAR_BASE:{
                    contornaEsq();
                    if(base==0){
                        awayStopAtLP();
                        xSeg=1;
                        while(analog(SENSOR_LUZ_ESQ)>luzEsq && analog(SENSOR_LUZ_ESQ)>30 && contador<60.0 && digital(SENSOR_BLOCO)==1){
                            retoXSeg();
                            contornaEsq();
                            stopAtLP();
                            xSeg=0;
                        }
                        if(contador<60.0){
                            pequenaRe();
                            gira90Dir();
                            gira90Dir();
                        }else{
                            para();
                        }
                    }
                    else{
                        awayStopAtRP();
                        xSeg=1;
                        while(analog(SENSOR_LUZ_DIR)>luzDir && analog(SENSOR_LUZ_DIR)>30 && contador<60.0 && digital(SENSOR_BLOCO)==1){
                            retoXSeg();
                            contornaEsq();
                            stopAtRP();
                            xSeg=0;
                        }
                        if(contador<60.0){
                            pequenaRe();
                            gira90Esq();
                            gira90Esq();
                        }else{
                            para();
                        }
                    }
                    
                    break;
                }
                
                case CUSPIR:
                  pequenaRe();
                pequenaRe();
                gira90Dir();
                break;
                
                
                default:
                  pequenaRe();
                pequenaRe();
                gira90Dir();
                
                break;
            }
            
            garantia=0.0;
            pid=start_process(andaLinhaEscura());
        }
    }
    
    kill_process(pid2);
    kill_process(pid);
}

void retoXSeg(){  //9 pulsos anda 2.3s.
    //xSeg = 0,1,2,3,4 e 5 = 2, 4, 6, 8, 10 e 12s;
    int i=(xSeg+1)*9; //9, 18, 27...
    pulsosEsq=0;
    pulsosDir=0;
    while(start_button());
    
    reto();
    movimento=RETO;
    while((pulsosEsq<i || pulsosDir<i) && contador<60.0){
        if(pulsosEsq < pulsosDir)      retoEndireitando();
        else if(pulsosEsq > pulsosDir) retoEsquerdando();
        else                           reto();
        printf("ESQ=%d DIR=%d      fim=%d\n", pulsosEsq, pulsosDir, i);
    }
    para();
    movimento=PARADO;
}

void afastaLuz(){
    if(luzEsq < luzDir){ base=0; afastaLuzLP();}
    else               { base=1; afastaLuzRP();}
}

///// ///// ///// ///// MAIN ///// ///// ///// /////
void main(){
    int pid=0;
    movimento=PARADO;
    contador = 0.0;
    
    xSeg = 0; //=2s
    
    start_process(contaPulsosBBeams());
    
    //loop principal
    while(1){
        //cor      R       G       B
        //amarelo   80-130 135-185  90-120
        //vermelho 110-165 175-220 120-170
        //verde    165-200 175-220 100-150
        //azul     165-200 220-235  75-135
        
        if(start_button() && movimento == PARADO){
            pulsosEsq=0;
            pulsosDir=0;
            calibraLuz();
            
            movimento = STANDBY;
        }
        if(start_button() && movimento == STANDBY){
            while(e==0 || d==0 || aux==0){
                printf("e=%d d=%d a=%d aguardando luz\n", e, d, aux);
                sleep(0.2);
                leLinha();
            }
            
            pid = start_process(contaTempo());
            afastaLuz();
            retoXSeg();
            retoAteLinha();
            
            obedeceBloco();
        }
        
        if(     verm<130  && azul<120) printf("YEL ");
        else if(verm<165  && azul>120)   printf("RED ");
        else if(verm>165  && verd<220)   printf("GRE ");
        else if(verm>165  && verd>220)   printf("BLU ");
        else                             printf("... ");
        
        //printf("R%d G%d B%d LP%d RP%d\n", verm, verd, azul, luzEsq, luzDir);
        printf("e=%d d=%d a=%d\n", e, d, aux);
        
        sleep(0.2);
        leSensoresCor();
        leLinha();
        
        if(contador >= 60.0){
            printf("Acabou o tempo!\n");
            para();
            break;
        }
    }
}