//defines e variaveis programa 1 - localizacao
#define DIF_SENSOR 3
#define NUM_VAL_LUZ 10
#define TOQUE_FRENTE 13
#define POTENCIA_MOTOR 60
#define POTENCIA_MOTOR_DIR 60
#define POTENCIA_MOTOR_ESQ 80
#define POTENCIA_ROTACAO 55
//#define BOUND_LUZ2 80
//#define BOUND_LUZ1 160
#define MIN_LUZ1 132
#define MAX_LUZ2 123
/*maquina de estados
0 - perdido
1 - totalmente perdido
2 - caminhando ao objetivo
*/

//valores proximos e distantes das fontes de luz
//persistent int maxLuz1, minLuz1, maxLuz2, minLuz2;
persistent int BOUND_LUZ2;
persistent int BOUND_LUZ1;
persistent int BOUND_LUZ1_ENCOSTADO;
persistent int BOUND_LUZ2_ENCOSTADO;
int ultimosValoresLuz[NUM_VAL_LUZ];
int terminou;


//defines e variaveis programa 2 - segue linha
#define ROT_90 15
#define TOQUE_FRENTE 13
#define POTENCIA_MOTOR 60
#define POTENCIA_ROTACAO 55
#define LIGHT_SENSOR_DIR 6
#define LIGHT_SENSOR_ESQ 4
#define COLOR_SENSOR 2
#define DIR 0
#define LEFT 1
#define MOTOR_DIR 0
#define MOTOR_ESQ 1
#define ENCODER_DIR 0
#define ENCODER_ESQ 1
/*
estado 0 -> 00 -> nenhum sensor ve a linha
estado 1 -> 01 -> somente o da direita ve a linha
estado 2 -> 10 -> somente o da esquerda ve a linha
estado 3 -> 11 -> ambos os sensores veem a linha
*/
int estado, ultimoEstado = -1;
float UpperBoundEsq, UpperBoundDir, LowerBoundEsq, LowerBoundDir;

int objetoDetectado = -1;

persistent int numColors;
persistent int colorValues[40][3];
persistent int tempoReta;

#define COLOR_SENSOR 2
//fio amarelo = verde
//fio laranja = azul
//fio vermelho = vermelho
#define RED_LED 0x08
#define GREEN_LED 0x06
#define BLUE_LED 0xA0
#define PESO_DIST 1
#define PESO_IND 0



//defines e variaveis do programa 3 - linha reta
#define POT_MIN 60
#define POT_MAX 100

#define Kp 2.0
#define Kd 0.8


#use "musictmpl.c"


int main(){
    int opcao, irUsado=0;
    //prompt da opcao
    enable_encoder(ENCODER_DIR);    
    enable_encoder(ENCODER_ESQ);
    //    sony_init(1);
    //seta leds apagados
    poke(0x1008, 0x00);
    printf("\n\nSelecione opcao desejada");
    sleep(3.0);
    while(!start_button()){
        //opcao por IR
        //        opcao = ir_data(0);
        if(opcao >2 && opcao <6){
            opcao -= 2;
            irUsado = 1;
            break;
        }
        
        //normaliza para valores entre 1 e 3
        opcao = knob()*2/255 +1;
        if(opcao == 1){
            printf("\n\n1 - localizacao");
        }
        else if(opcao == 2){
            printf("\n\n2 - segue linha");
        }
        else{
            printf("\n\n3 - linha reta");
        }
        
        
        sleep(0.2);
    }
    if(!irUsado){
        while(start_button());
    }
    
    
    //    sony_init(0);
    
    if(opcao == 1){
        Programa1_Localizacao();
    }
    else if(opcao == 2){
        Programa2_SegueLinha();
    }
    else{
        Programa3_LinhaReta();
    }
    
}


//secao localizacao

void calibraPolarizacao(){
    int bound1, bound2,opcaoIR;
    printf("\n\nColoque o robo na luz");
    while(!start_button()){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            break;
        }
    }
    while(start_button());
    //start_press();
    beep();
    bound1 = getMediaLightValue(DIF_SENSOR);
    printf("\n\nvalor: %d", bound1);
    sleep(2.0);
    printf("\n\nColoque o robo na outra luz");
    while(!start_button()){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            break;
        }
    }
    while(start_button());
    //start_press();
    beep();
    bound2 = getMediaLightValue(DIF_SENSOR);
    printf("\n\nvalor: %d", bound2);
    sleep(2.0);
    if(bound1 > bound2){
        BOUND_LUZ1_ENCOSTADO = bound1 -(bound1 - 127)/5;
        BOUND_LUZ2_ENCOSTADO = bound2 +(127 - bound2)/5;
        bound1 -= (bound1 - 127)/10;
        bound2 += (127 - bound2)/10;
        BOUND_LUZ1 = bound1;
        BOUND_LUZ2 = bound2;
    }
    else{
        BOUND_LUZ1_ENCOSTADO = bound2 -(bound2 - 127)/5;
        BOUND_LUZ2_ENCOSTADO = bound1 +(127 - bound1)/5;
        bound2 -= (bound2 - 127)/10;
        bound1 += (127 - bound1)/10;
        BOUND_LUZ1 = bound2;
        BOUND_LUZ2 = bound1;
    }
}

void Programa1_Localizacao(){
    int pidLocaliza, pidToque;
    int escolhido,opcaoIR;
    //habilita receptor IR
    //    sony_init(1);
    printf("\n\nDeseja calibrar polarizacao?");
    while(1){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            calibraPolarizacao();
            break;
        }
        else if(opcaoIR == 21){
            break;
        }
        if(start_button()){
            while(start_button());
            calibraPolarizacao();
            break;
        }
        else if(stop_button()){
            while(stop_button());
            break;
        }
        sleep(0.2);
    }
    
    printf("\n\nstart- luz 1 stop- 2");
    terminou = 0;
    while(1){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 1){
            escolhido = 1;
            break;
        }
        else if(opcaoIR == 2){
            escolhido = 2;
            break;
        }
        if(start_button()){
            while(start_button());
            escolhido = 1;  
            break;
        }
        else if(stop_button()){
            while(stop_button());
            escolhido = 2; 
            break;
        }
    }
    
    enable_encoder(ENCODER_DIR);    
    enable_encoder(ENCODER_ESQ);
    
    pidLocaliza = start_process(Localiza(escolhido),5);
    //pidToque = start_process(MonitoraSensoresToque(),1);
    //start_process(CondFim(pidLocaliza));
    //espera botao stop ou termino do algoritmo
    while(!stop_button() && !terminou){
        //        opcaoIR = ir_data(0);
        //printf("\n\nrec %d",opcaoIR);
        if(opcaoIR == 21){
            break;
        }
        sleep(0.1);
    }
    kill_process(pidLocaliza);
    ao();
    if(opcaoIR == 21){
        printf("\n\nInterrompido =/");
        //tone(0.2,1.0);
        beeper_on();
        sleep(1.0);
        beeper_off();
    }
    
    //    sony_init(0);
    
    //kill_process(pidToque);
    //Localiza(escolhido);
    
}
/*
void CondFim(int pidLocaliza){
    int opcaoIR;
    sony_init(1);
    while(!stop_button() && !terminou){
        opcaoIR = ir_data(0);
        if(opcaoIR == 21){
            break;
        }
    }
    if(opcaoIR == 21){
        tone(0.2,1.0);
    }
    kill_process(pidLocaliza);
}*/

void Localiza(int escolhido){
    
    do{
        
        beep();
        sleep(1.0);
        printf("\n\nEscolhendo direcao...");
        
        
        //estado 0 - localiza fonte escolhida
        if(!LocalizaFonte360(escolhido)){
            //estado 1 - totalmente perdido
            do{
                printf("\n\nDirecao nao encontrada...");
                beep();
                sleep(1.0);
                printf("\n\nProcurando fonte de luz...");
                MovimentoAleatorio(escolhido);
                printf("\n\nFonte achada!");
                beep();
                sleep(1.0);
                printf("\n\nEscolhendo direcao...");
            }while(!LocalizaFonte360(escolhido));
        }
        
        beep();
        sleep(1.0);
        printf("\n\nCaminhando a fonte...");
        
        //estado 2 - caminhando ate fonte escolhida
    }while(!CaminhaAteFonte(escolhido));
    
    beep();
    printf("\n\nChegou!");
    sleep(0.5);
    beep();
    LocalizaFonte360(escolhido);
    //beep();
    charge();
    //chegou a fonte de luz escolhida
    terminou = 1;
    
}


int CaminhaAteFonte(int escolhido){
    
    int luz;
    int index;
    int i;
    int erro, chegou;
    index = 0;
    erro = 0;
    //inicializa ultimos val de luz
    for(i=0;i<NUM_VAL_LUZ;i++){
        ultimosValoresLuz[i] = -1;
    }
    
    motor(MOTOR_DIR,POTENCIA_MOTOR_DIR);
    motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
    while(erro == 0){
        luz = getMediaLightValue(DIF_SENSOR);
        ultimosValoresLuz[index] = luz;
        erro = 1;
        if(escolhido == 1){
            //condicao de saida do estado 2 - se todos os ultimos valores forem maiores que o atual
            for(i=1;i<NUM_VAL_LUZ;i++){
                if(ultimosValoresLuz[(index+i)%NUM_VAL_LUZ] <= luz || ultimosValoresLuz[(index+i)%NUM_VAL_LUZ] == -1){
                    erro = 0;
                }
            }
        }
        else{
            //condicao de saida do estado 2 - se todos os ultimos valores forem menores que o atual
            for(i=1;i<NUM_VAL_LUZ;i++){
                if(ultimosValoresLuz[(index+i)%NUM_VAL_LUZ] >= luz || ultimosValoresLuz[(index+i)%NUM_VAL_LUZ] == -1){
                    erro = 0;
                }
            }
        }
        sleep(0.1);
        index++;
        index = index%NUM_VAL_LUZ;
        
        //se chegar a fonte de luz
        /*if(toqueDir == 1 || toqueEsq == 1){
            ao();
            return 1;
        }*/
        
        if(digital(TOQUE_FRENTE)){
            ao();
            if(escolhido == 1){
                if(luz > BOUND_LUZ1_ENCOSTADO) return 1;
                else{
                    motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                    motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                    sleep(1.0);
                    ao();
                    return 0;
                }
            }
            else{
                if(luz < BOUND_LUZ2_ENCOSTADO) return 1;
                else{
                    motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                    motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                    sleep(1.0);
                    ao();
                    return 0;
                }
            }
        }
        
        chegou = 1;
        for(i=0;i<NUM_VAL_LUZ;i++){
            if(escolhido == 1){
                if(ultimosValoresLuz[i] < BOUND_LUZ1){
                    chegou = 0;
                }
            }
            else{
                if(ultimosValoresLuz[i] > BOUND_LUZ2){
                    chegou = 0;
                }
            }
        }
        if(chegou){
            ao();
            return 1;
        }
        /*if(luz > BOUND_LUZ1 && escolhido == 1 || luz < BOUND_LUZ2 && escolhido == 2){
            ao();
            return 1;
        }*/
        
    }
    ao();
    return 0;
    
}

//realiza espiral quadrada enquanto monitora sensor diferencial
//quando vLuz < MAX_LUZ2 ou > MIN_LUZ1, vai p/ estado 0
void MovimentoAleatorio(int escolhido){
    int c, luz, cond,i;
    c=1;
    cond = 0;
    //procura fazendo quadrados
    luz = getMediaLightValue(DIF_SENSOR);
    while(luz > MAX_LUZ2 && escolhido == 2 ||  luz < MIN_LUZ1 && escolhido == 1){
        for(i=0;i<3;i++){
            motor(MOTOR_ESQ, POTENCIA_MOTOR_ESQ);
            motor(MOTOR_DIR, POTENCIA_MOTOR_DIR);
            reset_system_time();
            while(seconds() < (float)c*0.5){
                luz = getMediaLightValue(DIF_SENSOR);
                if(luz < MAX_LUZ2 && escolhido == 2 ||  luz > MIN_LUZ1 && escolhido == 1){
                    cond = 1;
                    break;
                }
                if(digital(TOQUE_FRENTE)){
                    ao();
                    if(escolhido == 1){
                        motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                        motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                        sleep(1.0);
                        ao();
                        //Rotate90(0);
                    }
                    else{
                        motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                        motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                        sleep(1.0);
                        ao();
                        //Rotate90(0);
                    }
                }
            }
            if(cond == 1){
                ao();
                break;
            }
            //rotacao
            reset_encoder(ENCODER_DIR);
            reset_encoder(ENCODER_ESQ);
            motor(MOTOR_DIR,-POTENCIA_ROTACAO);
            motor(MOTOR_ESQ,POTENCIA_ROTACAO);
            //reset_system_time();
            while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90){
                luz = getMediaLightValue(DIF_SENSOR);
                if(luz < MAX_LUZ2 && escolhido == 2 ||  luz > MIN_LUZ1 && escolhido == 1){
                    cond = 1;
                    break;
                }
            }
            if(cond == 1){
                ao();
                break;
            }
        }
        if(cond == 1) break;
        c++;
        luz = getMediaLightValue(DIF_SENSOR);
    }
    ao();
    
}

int LocalizaFonte360(int escolhido){
    int melhor, valorRot;
    int luz;
    //gira p/ a posicao que maximiza ou minimiza Light
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    melhor = getMediaLightValue(DIF_SENSOR);
    valorRot = 0;
    motor(MOTOR_DIR,POTENCIA_ROTACAO);
    motor(MOTOR_ESQ,-POTENCIA_ROTACAO);
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90*4){
        luz = getMediaLightValue(DIF_SENSOR);
        if(escolhido == 1){
            if(luz > melhor){
                melhor = luz;
                valorRot = read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ);
            }
        }
        else{
            if(luz < melhor){
                melhor = luz;
                valorRot = read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ);
            }
        }
    }
    ao();
    //se o valor diferencial for baixo
    if(melhor > MAX_LUZ2 && melhor < MIN_LUZ1) return 0;
    
    valorRot = read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) - valorRot;
    //valorRot -= read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ);
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    motor(MOTOR_DIR,-POTENCIA_ROTACAO);
    motor(MOTOR_ESQ,POTENCIA_ROTACAO);
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < valorRot);
    ao();
    return 1;
    
}




//secao segue linha


void Programa2_SegueLinha(){
    int pidSensors;
    int pidMotors;
    int pidColors;
    int pidSegueLinha,opcaoIR;
    estado=0;
    //habilita saidas digitais
    poke(0x1009, 0x3c);
    //calibracao
    //    sony_init(1);
    printf("\n\nDeseja calibrar as cores?");
    while(1){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            numColors = 4;
            calibrationColors(COLOR_SENSOR);
            break;
        }
        else if(opcaoIR == 21){
            break;
        }
        if(start_button()){
            while(start_button());
            /*printf("\n\nSelecione o numero de cores");
            sleep(2.0);
            while(!start_button()){
                numColors = knob()*39/255 +1;
                printf("\n\n");
                printf("%d cores",numColors);
                sleep(0.1);
            }
            while(start_button());*/
            numColors = 4;
            calibrationColors(COLOR_SENSOR);
            break;
        }
        else if(stop_button()){
            while(stop_button());
            break;
        }
    }
    
    calibration();
    printf("\n\nColoque o carro na linha e de start");
    while(!start_button()){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            break;
        }
    }
    while(start_button());
    //start_press();
    
    
    printf("\n\nExecutando...");
    pidSensors = start_process(MonitorSensors(),5);
    pidMotors = start_process(FollowLine(),5);
    //pidSegueLinha = start_process(SegueLinha(),5);
    pidColors = start_process(MonitorColors(),5);
    while(!stop_button()){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 21){
            break;
        }
    }
    while(stop_button());
    //kill_process(pidSegueLinha);
    kill_process(pidSensors);
    kill_process(pidMotors);
    kill_process(pidColors);
    //seta leds apagados
    poke(0x1008, 0x00);
    ao();
    printf("\n\nEncerrado =/");
    //tone(0.2,1.0);
    beeper_on();
    sleep(1.0);
    beeper_off();
    //    sony_init(0);
    //stop_press();
    
    
}


void calibration(){
    int opcaoIR;
    float difDir, difEsq;
    printf("\n\nColoque o carro na linha e aperte start");
    while(!start_button()){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            break;
        }
    }
    while(start_button());
    //while(!start_button());
    //while(start_button());
    printf("\n\nLendo valor da linha...");
    LowerBoundDir = getMediaLightValueF(LIGHT_SENSOR_DIR);
    LowerBoundEsq = getMediaLightValueF(LIGHT_SENSOR_ESQ);
    
    //aumenta ligeiramente o valor do limite inferior
    //LowerBoundDir = LowerBoundDir*1.2;
    //LowerBoundEsq = LowerBoundEsq*1.2;
    
    beep();
    printf("\n\nEsq: %f Dir: %f", LowerBoundEsq, LowerBoundDir);
    sleep(3.0);
    
    printf("\n\nColoque o carro fora da linha e aperte start");
    while(!start_button()){
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            break;
        }
    }
    while(start_button());
    //while(!start_button());
    //while(start_button());
    printf("\n\nLendo valor fora da linha...");
    
    UpperBoundDir = getMediaLightValueF(LIGHT_SENSOR_DIR);
    UpperBoundEsq = getMediaLightValueF(LIGHT_SENSOR_ESQ);
    
    //reduz ligeiramente o valor do limite superior
    //UpperBoundDir = UpperBoundDir*0.9;
    //UpperBoundEsq = UpperBoundEsq*0.9;
    beep();
    printf("\n\nEsq: %f Dir: %f", UpperBoundEsq, UpperBoundDir);
    sleep(3.0);
    
    //ajusta bounds
    difDir = UpperBoundDir - LowerBoundDir;
    difEsq = UpperBoundEsq - LowerBoundEsq;
    UpperBoundDir -= difDir/5.0;
    LowerBoundDir += difDir/5.;
    UpperBoundEsq -= difEsq/5.0;
    LowerBoundEsq += difEsq/5.0;
    
    printf("\n\nCalibracao concluida!");
    sleep(2.0);
    
}

int SegueLinha(){
    while(1){
        do{
            MonitorSensors();
        }while(ultimoEstado == estado && objetoDetectado == -1);
        FollowLine();
    }
}

int MonitorSensors(){
    float valueDir, valueEsq;
    
    ultimoEstado = estado;
    
    while(1){
        valueDir = getMediaLightValueF(LIGHT_SENSOR_DIR);
        valueEsq = getMediaLightValueF(LIGHT_SENSOR_ESQ);
        
        
        //condicoes com histerese
        if((valueDir > UpperBoundDir || valueDir > LowerBoundDir && (estado == 0 || estado == 2)) && (valueEsq > UpperBoundEsq || valueEsq > LowerBoundEsq && estado < 2)){
            //ambos nao enxergam a linha preta
            estado = 0;
        }
        else if((valueDir <= LowerBoundDir || valueDir < UpperBoundDir && (estado == 1 || estado == 3))  && (valueEsq > UpperBoundEsq || valueEsq > LowerBoundEsq && estado <2)){
            //somente o da direita ve a linha
            estado = 1;
        }
        else if((valueDir > UpperBoundDir || valueDir > LowerBoundDir && (estado == 2 || estado == 0) ) && (valueEsq <= LowerBoundEsq || valueEsq < UpperBoundEsq && estado >1)){
            //somente o da esquerda ve a linha
            estado = 2;
        }
        else if((valueDir <= LowerBoundDir || valueDir < UpperBoundDir && (estado == 1 || estado == 3)) && (valueEsq <= LowerBoundEsq || valueEsq < UpperBoundEsq && estado > 1)){
            //ambos enxergam a linha preta
            estado = 3;
        }
        
    }
    
}


int FollowLine(){
    int valorDir, valorEsq;
    int ultimo0 = 0;
    int i,c;
    float secAux;
    
    while(1){
        
        //acoes por deteccao de blocos
        if(objetoDetectado == -1){
            
            if(digital(TOQUE_FRENTE)){
                beep();
                motor(MOTOR_DIR,-POTENCIA_ROTACAO);
                motor(MOTOR_ESQ,-POTENCIA_ROTACAO);
                sleep(0.5);
                ao();
                reset_encoder(ENCODER_DIR);
                reset_encoder(ENCODER_ESQ);
                motor(MOTOR_DIR,-POTENCIA_ROTACAO);
                motor(MOTOR_ESQ,POTENCIA_ROTACAO);
                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < 3*ROT_90 && estado != 0);
                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < 3*ROT_90 && estado == 0);
                ao();
            }
            
            //maquina de estados
            switch(estado){
                
                case 3:
                  valorDir = POTENCIA_MOTOR_DIR;
                valorEsq = POTENCIA_MOTOR_ESQ;
                break;
                
                case 2:
                  valorDir = POTENCIA_MOTOR_DIR;
                valorEsq = 0;
                break;
                
                case 1:
                  valorDir = 0;
                valorEsq = POTENCIA_MOTOR_ESQ;
                break;
                
                default:
                  //rotacao 90 graus
                  beep();
                reset_encoder(ENCODER_DIR);
                reset_encoder(ENCODER_ESQ);
                motor(MOTOR_DIR,POTENCIA_ROTACAO);
                motor(MOTOR_ESQ,-POTENCIA_ROTACAO);
                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90 && estado == 0);
                //ao();
                if(estado == 0){
                    beep();
                    //rotacao 360 graus
                    reset_encoder(ENCODER_DIR);
                    reset_encoder(ENCODER_ESQ);
                    motor(MOTOR_DIR,-POTENCIA_ROTACAO);
                    motor(MOTOR_ESQ,POTENCIA_ROTACAO);
                    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < 4*ROT_90 && estado == 0);
                    ao();
                    if(estado == 0){
                        beep();
                        printf("\n\nProcurando linha...");
                        c=1;
                        //procura fazendo quadrados
                        while(estado == 0){
                            for(i=0;i<3;i++){
                                motor(MOTOR_ESQ, POTENCIA_MOTOR);
                                motor(MOTOR_DIR, POTENCIA_MOTOR);
                                reset_system_time();
                                while(estado == 0 && seconds() < (float)c*0.5){
                                    if(digital(TOQUE_FRENTE)){
                                        ao();
                                        motor(MOTOR_DIR,-POTENCIA_MOTOR);
                                        motor(MOTOR_ESQ,-POTENCIA_MOTOR);
                                        secAux = seconds();
                                        while(estado==0 && seconds() < secAux+1.0);
                                        //sleep(1.0);
                                        ao();
                                    }
                                }
                                if(estado != 0){
                                    ao();
                                    break;
                                }
                                //rotacao
                                reset_encoder(ENCODER_DIR);
                                reset_encoder(ENCODER_ESQ);
                                motor(MOTOR_DIR,-POTENCIA_ROTACAO);
                                motor(MOTOR_ESQ,POTENCIA_ROTACAO);
                                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90 && estado == 0);
                                if(estado != 0){
                                    ao();
                                    break;
                                }
                            }
                            c++;
                        }
                        printf("\n\nLinha achada!");
                        beep();
                    }
                }
                ultimo0 = 1;
                
            }
            if(ultimo0 != 1){
                motor(MOTOR_ESQ,valorEsq);
                motor(MOTOR_DIR,valorDir);
            }
            ultimo0 = 0;
            
            
        }
        
        else{
            //duvida se fica aqui ou tira
            hog_processor();
            
            switch(objetoDetectado){
                case 0:
                  printf("Objeto 0 - vira DIR");
                Rotate90(DIR);
                break;
                
                case 1:
                  printf("Objeto 1 - vira ESQ");
                Rotate90(LEFT);
                break;
                
                case 2:
                  printf("Objeto 2 - Para");
                ao();
                break;
                
                case 3:
                  printf("Objeto 3 - Andar Reto");
                motor(MOTOR_DIR,POTENCIA_MOTOR_DIR);
                motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
                reset_system_time();
                while(seconds() < (float)tempoReta);
                ao();
            }
            //enable(pid);
            objetoDetectado = -1;
            //disable(pid);
            sleep(5.0);
            defer();
        }
    }
    
    
}

int MonitorColors(){
    int i,dist, menorDist,c,index;
    float distZero;
    int comObjeto=0;
    float ind, menorInd,mixInd, minMixInd;
    int valor[3];
    int ultimasCores[3];
    int ultimoIndex, detectado;
    
    for(c=0;c<3;c++){
        ultimasCores[c] = -1;
    }
    
    //execucao do fluxo principal
    while(1){
        do{
            if(comObjeto == -1){
                //printf("\n\nno object");
                //printf("\n\nObject not detected, try again!");
                sleep(0.1);
                comObjeto = 0;
            }
            //start_press();
            comObjeto = getRGB(COLOR_SENSOR,valor);
        }while(comObjeto == -1);
        
        //primeiro valor setado como "mais proximo"
        menorInd=0.;
        menorInd += square((float)valor[0]/(float)valor[1] - (float)colorValues[0][0]/(float)colorValues[0][1]);
        menorInd += square((float)valor[0]/(float)valor[2] - (float)colorValues[0][0]/(float)colorValues[0][2]);
        menorInd += square((float)valor[1]/(float)valor[2] - (float)colorValues[0][1]/(float)colorValues[0][2]);
        //index2 = 0;
        
        menorDist = 0;
        for(c=0;c<3;c++){
            menorDist += squareI(valor[c]-colorValues[0][c]);
        }
        minMixInd = pow((float)menorDist,PESO_DIST)*pow(menorInd+1.0,PESO_IND);
        //minMixInd = (float)menorDist;
        index = 0;
        
        for(i=1;i<numColors;i++){
            ind = 0.;
            ind += square((float)valor[0]/(float)valor[1] - (float)colorValues[i][0]/(float)colorValues[i][1]);
            ind += square((float)valor[0]/(float)valor[2] - (float)colorValues[i][0]/(float)colorValues[i][2]);
            ind += square((float)valor[1]/(float)valor[2] - (float)colorValues[i][1]/(float)colorValues[i][2]);
            dist = 0;
            for(c=0;c<3;c++){
                dist += squareI(valor[c]-colorValues[i][c]);
            }
            mixInd = pow((float)dist,PESO_DIST)*pow(ind+1.0,PESO_IND);
            //mixInd = (float)dist;
            if(mixInd < minMixInd){
                minMixInd = mixInd;
                index = i;
            }
        }
        
        distZero = 0.;
        for(c=0;c<3;c++){
            distZero += square((float)valor[c]);
        }
        
        if(distZero < minMixInd){
            
        }
        else{
            
            //seta a cor calculada como mais proxima
            //printf("\n\ncor %d",index+1);
            /*ultimoIndex++;
        ultimoIndex = ultimoIndex % 3;
        ultimasCores[ultimoIndex] = index;
        detectado = 1;
        for(c=0;c<3;c++){
            if(ultimasCores[c] != ultimoIndex){
                detectado = 0;
            }
        }*/
            printf("\n\nerro %f",minMixInd);
            sleep(0.1);
            detectado = 1;
            if(detectado){
                hog_processor();
                //enable(pid);
                objetoDetectado = index;
                //reseta ultimas cores
                for(c=0;c<3;c++){
                    ultimasCores[c] = -1;
                }
                //disable(pid);
                defer();
            }
        }
        
        
    }
    
}

int getRGB(int sensor, int valor[]){
    //filtro de luz ambiente
    int vSemLuz = getMediaLightValue(sensor);
    valor[0] = getLedRGB(sensor,RED_LED) - vSemLuz;
    valor[1] = getLedRGB(sensor,GREEN_LED) - vSemLuz;
    valor[2] = getLedRGB(sensor,BLUE_LED)- vSemLuz;
    //garante intervalo entre o proximo calculo de luz
    sleep(0.1);
    //notifica que nao detectou objeto
    if(valor[0] < 5 && valor[1] < 5 && valor[2] < 5) return -1;
    else if(valor[0] < 1 || valor[1] < 1 || valor[2] < 1) return -1;
    
    else return 0;
}

int getLedRGB(int sensor, int led){
    int value;
    
    poke(0x1008, led);
    sleep(0.1);
    value = getMediaLightValue(sensor);
    //sleep(1.0);
    poke(0x1008, 0x00);
    
    //printf("\n\nv1: %d v2: %d v3: %d",values[0],values[1],values[2]);
    
    return value;
}


void calibrationColors(int port){
    int valor[3];
    int repetir = 0, comObjeto = 0;
    int i,c,opcaoIR;
    /*char cores[6][];
    cores[0] = "preto";
    cores[1] = "vermelho";
    cores[2] = "azul";
    cores[3] = "verde";
    cores[4] = "amarelo";
    cores[5] = "marrom";
    //= {"preto","vermelho","azul","verde","amarelo"};
 */
    
    for(i=0; i< numColors ;i++){
        do{
            do{
                if(comObjeto == -1){
                    printf("\n\nObject not detected, try again!");
                    comObjeto = 0;
                }
                else{ if(repetir == 1){
                        printf("\n\nColoque objeto %d novamente",i+1);
                    }
                    else printf("\n\nColoque objeto %d e de start",i+1);
                }
                while(!start_button()){
                    //                    opcaoIR = ir_data(0);
                    if(opcaoIR == 61){
                        break;
                    }
                }
                while(start_button());
                //start_press();
                comObjeto = getRGB(port,valor);
                
            }while(comObjeto == -1);
            printf("\n\nr:%d g:%d b:%d. Accept?",valor[0],valor[1],valor[2]);
            while(1){
                //                opcaoIR = ir_data(0);
                if(opcaoIR == 61){
                    for(c=0;c<3;c++){
                        colorValues[i][c] = valor[c];
                    }
                    repetir = 0;
                    break;
                }
                else if(opcaoIR == 21){
                    repetir = 1;
                    break;
                }
                if(start_button()){
                    while(start_button());
                    for(c=0;c<3;c++){
                        colorValues[i][c] = valor[c];
                    }
                    repetir = 0;
                    break;
                }
                else if(stop_button()){
                    while(stop_button());
                    repetir = 1;
                    break;
                }
            }
        }while(repetir == 1);
    }
    
    //prompt do tempo
    printf("\n\nEscolha tempo linha reta");
    sleep(3.0);
    while(!start_button()){
        //normaliza para valores entre 1 e 30
        tempoReta = (knob()*29/255 +1);
        if(tempoReta == 1){
            printf("\n\n1 segundo");
        }
        else printf("\n\n%d segundos",tempoReta);
        
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            break;
        }
        sleep(0.1);
    }
    while(start_button());
    
}


//Secao Linha Reta


void Programa3_LinhaReta()
{
    int opcaoIR, tempo;
    int corrValue;
    long tempoL;
    int countEsq, countDir, erro, erroAnterior, oscila=0, ePositivo;
    int Potencia_Esq;
    int Potencia_Dir;
    float controlV; //manipulated variable
    float razao;
    float vTempo = 0.05;
    
    //    sony_init(1);
    //prompt do tempo
    printf("\n\nEscolha o tempo - press start");
    sleep(3.0);
    while(!start_button()){
        //normaliza para valores entre 1 e 30
        tempo = (knob()*29/255 +1);
        if(tempo == 1){
            printf("\n\n1 segundo");
        }
        else printf("\n\n%d segundos",tempo);
        
        //        opcaoIR = ir_data(0);
        if(opcaoIR == 61){
            break;
        }
        
        msleep(100L);
    }
    while(start_button());
    
    printf("\n\nIniciando...");
    sleep(2.0);
    
    //seta inicio
    enable_encoder(ENCODER_DIR);    
    enable_encoder(ENCODER_ESQ);
    Potencia_Esq = 70;
    Potencia_Dir = 70;
    reset_encoder(ENCODER_ESQ);
    reset_encoder(ENCODER_DIR);
    reset_system_time();
    tempo = tempo*1000;
    tempoL = (long) tempo;
    erroAnterior = 0;
    //executa trajetoria em linha reta, durante tempo determinado
    while(mseconds() < tempoL && !stop_button()){
        if(Potencia_Esq < POT_MIN) Potencia_Esq = POT_MIN;
        if(Potencia_Dir < POT_MIN) Potencia_Dir = POT_MIN;
        if(Potencia_Esq > POT_MAX) Potencia_Esq = POT_MAX;
        if(Potencia_Dir > POT_MAX) Potencia_Dir = POT_MAX;
        printf("\n\npot %d %d count %d %d",Potencia_Esq,Potencia_Dir,countEsq,countDir);
        motor(MOTOR_ESQ,Potencia_Esq);
        motor(MOTOR_DIR,Potencia_Dir);
        //reset_encoder(ENCODER_ESQ);
        //reset_encoder(ENCODER_DIR);
        sleep(vTempo);
        //condicao inicial para evitar divisao por zero
        while(read_encoder(ENCODER_DIR) < 1 || read_encoder(ENCODER_ESQ) < 1);
        countDir = read_encoder(ENCODER_DIR);
        countEsq = read_encoder(ENCODER_ESQ);
        //printf("\n\ndir: %d esq: %d",countDir,countEsq);
        ao();
        erro = countDir - countEsq;
        if(erro < 0){
            erro*= -1;
            ePositivo = -1;
        }
        else{
            if(erro > 0){ ePositivo = 1;}
            else ePositivo = 0;
        }
        //corrige potencia para que as rodas girem o mesmo tanto
        /*razao = (float)countDir / (float)countEsq;
        Potencia_Esq = (int)((float)Potencia_Esq*sqrt(razao));
        Potencia_Dir = (int)((float)Potencia_Dir/sqrt(razao));*/
        
        //corrige potencia para que o erro seja compensado pela roda que girou menos
        //controlV = Kp*(float)erro - Kd*((float)(erroAnterior - erro));
        controlV = Kp*(float)erro - Kd*((float)(erroAnterior - erro));
        if(ePositivo == -1){
            //Potencia_Dir *= (Kp*(erro/countEsq) + 1);
            //Potencia_Dir = Potencia_Dir + controlV;
            //Potencia_Dir = (int)((float)Potencia_Dir*(Kp*((float)erro/(float)countEsq) + 1.0));
            if(controlV > 0. && (float)countEsq > controlV && (float)countDir > controlV){
                Potencia_Dir = (int)((float)Potencia_Dir/sqrt(((float)countDir - controlV)/(float)countDir));
                Potencia_Esq = (int)((float)Potencia_Esq*sqrt(((float)countDir - controlV)/(float)countDir));
                
                //corrValue = ((countDir - (int)controlV)/(countDir))*Potencia_Dir;
                //Potencia_Dir += corrValue/2;
                //Potencia_Esq -= corrValue/2;
            }
            //Potencia_Dir = (int)((float)Potencia_Dir*((controlV/(float)countEsq)));
        }
        else if(ePositivo == 1){
            //Potencia_Esq = (int)((float)Potencia_Esq*(Kp*((float)erro/(float)countDir) + 1.0));
            //Potencia_Esq = (int)((float)Potencia_Esq*((controlV/(float)countDir)));
            if(controlV > 0. && (float)countEsq > controlV && (float)countDir > controlV){
                //corrValue = ((countEsq - (int)controlV)/(countEsq))*Potencia_Esq;
                //Potencia_Dir -= corrValue/2;
                //Potencia_Esq += corrValue/2;
                Potencia_Dir = (int)((float)Potencia_Dir*sqrt(((float)countEsq - controlV)/(float)countEsq));
                Potencia_Esq = (int)((float)Potencia_Esq/sqrt(((float)countEsq - controlV)/(float)countEsq));
            }
        }
        
        erroAnterior = erro;
        
    }
    
    //termina
    ao();
    /*    if(ir_data(0) == 21){
       printf("\n\nInterrompido =/");
        //tone(0.2,1.0);
        beeper_on();
        sleep(1.0);
        beeper_off();
    }
    sony_init(0);*/
    
}

//utils

int getLightValue(int sensor){
    return 255 - analog(sensor);
}

int getMediaLightValue(int sensor){
    int valor=0;
    int i;
    for(i = 1; i <= 10; i++){
        valor += getLightValue(sensor);
    }
    return valor / 10;
}

float getMediaLightValueF(int sensor){
    float valor=0.0;
    int i;
    for(i = 1; i <= 10; i++){
        valor += (float)getLightValue(sensor);
    }
    return valor / 10.0;
}


void Rotate90(int sentido){
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    if(sentido == 0){
        motor(MOTOR_DIR,POTENCIA_ROTACAO);
        motor(MOTOR_ESQ,-POTENCIA_ROTACAO);
    }
    else{
        motor(MOTOR_DIR,-POTENCIA_ROTACAO);
        motor(MOTOR_ESQ,POTENCIA_ROTACAO);
    }
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90);
    ao();
}


float pow(float n, int expo){
    int i;
    float result=1.0;
    for(i=0;i<expo;i++){
        result *= n;
    }
    return result;
}

float square(float n){
    return n*n;
}

int squareI(int n){
    return n*n;
}