#use "calibracoes.ic"
int ultimosValoresLuz[NUM_VAL_LUZ];

void Localiza(int escolhido, int tipo){
    int sair=0;
    do{
        
        beep();
        printf("\n\nEscolhendo direcao...");
        
        
        //estado 0 - localiza fonte escolhida
        if(!LocalizaFonte360(escolhido,tipo)){
            //estado 1 - totalmente perdido
            do{
                if(tipo == 2 && mseconds() >= 53000L){
                    motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
                    msleep(4000L);
                    off(MOTOR_BLOCO);
                    Back(4);
                }
                printf("\n\nDirecao nao encontrada...");
                beep();
                if(carregandoObjeto == 1 && getMediaLightValue(BREAK_BEAM) < Bound_BB){
                    sair = 1;
                    break;
                }                                      
                printf("\n\nProcurando fonte de luz...");
                MovimentoAleatorio(escolhido,tipo);
                 if(carregandoObjeto == 1 && getMediaLightValue(BREAK_BEAM) < Bound_BB){
                    sair = 1;
                    break;
                }
                printf("\n\nFonte achada!");
                beep();
                printf("\n\nEscolhendo direcao...");
                if(tipo == 2 && mseconds() >= 53000L){
                    motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
                    msleep(4000L);
                    off(MOTOR_BLOCO);
                    Back(4);
                }
            }while(!LocalizaFonte360(escolhido,tipo));
        }
        if(carregandoObjeto == 1 && getMediaLightValue(BREAK_BEAM) < Bound_BB){
                    sair = 1;
                    break;
        }   
        if(sair) break;
        beep();
        printf("\n\nCaminhando a fonte...");
        if(tipo == 2 && mseconds() >= 53000L){
            motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
            msleep(4000L);
            off(MOTOR_BLOCO);
            Back(4);
        }
        //estado 2 - caminhando ate fonte escolhida
    }while(!CaminhaAteFonte(escolhido,tipo));
    
    beep();
    //printf("\n\nChegou!");
    //sleep(0.5);
    //beep();
    //LocalizaFonte360(escolhido);
    //beep();
    //charge();
    //chegou a fonte de luz escolhida
    
}


int CaminhaAteFonte(int escolhido, int tipo){
    
    int luz,pidEncoder;
    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;
    }
    pidEncoder = start_process(andaReto(60,100,60,100,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(tipo == 2 && mseconds() >= 53000L){
            kill_process(pidEncoder);
            motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
            msleep(4000L);
            off(MOTOR_BLOCO);
            Back(4);
        }
        
        if(digital(TOQUE_ESQ) || digital(TOQUE_DIR) || digital(TOQUE_FRENTE)){
            ao();
            if(escolhido == 1){
                if(luz > BOUND_LUZ1_ENCOSTADO){
                    kill_process(pidEncoder);
                    ao();
                    return 1;
                }
                else{
                    kill_process(pidEncoder);
                    Back(4);
                    //motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                    //motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                    //sleep(1.0);
                    //ao();
                    return 0;
                }
            }
            else{
                if(luz < BOUND_LUZ2_ENCOSTADO){
                    kill_process(pidEncoder);
                    ao();
                    return 1;
                }
                else{
                    kill_process(pidEncoder); 
                    Back(4); 
                    //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){
            kill_process(pidEncoder);
            ao();
            return 1;
        }
        /*if(luz > BOUND_LUZ1 && escolhido == 1 || luz < BOUND_LUZ2 && escolhido == 2){
            ao();
            return 1;
        }*/
        
    }
    kill_process(pidEncoder);
    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 tipo){
    int c, luz, cond,i;
    int pidEncoder;
    float tempoAtual;
    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++){
            if(tipo == 2 && mseconds() >= 53000L){
                motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
                msleep(4000L);
                off(MOTOR_BLOCO);
                Back(4);
            }
            pidEncoder = start_process(andaReto(60,100,60,100,1));
            //motor(MOTOR_ESQ, POTENCIA_MOTOR_ESQ);
            //motor(MOTOR_DIR, POTENCIA_MOTOR_DIR);
            //reset_system_time();
            tempoAtual = seconds();
            while(seconds() < tempoAtual + (float)c*2.5){
                luz = getMediaLightValue(DIF_SENSOR);
                if(luz < MAX_LUZ2 && escolhido == 2 ||  luz > MIN_LUZ1 && escolhido == 1){
                    cond = 1;
                    break;
                }
                if(digital(TOQUE_ESQ) || digital(TOQUE_DIR) || digital(TOQUE_FRENTE)){
                    ao();
                    if(escolhido == 1){
                        kill_process(pidEncoder);
                        Back(4);
                        pidEncoder = start_process(andaReto(60,100,60,100,1));
                        //motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                        //motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                        //sleep(1.0);
                        //ao();
                        //Rotate90(0);
                    }
                    else{
                        kill_process(pidEncoder);
                        Back(4);
                        pidEncoder = start_process(andaReto(60,100,60,100,1));
                        //motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                        //motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                        //sleep(1.0);
                        //ao();
                        //Rotate90(0);
                    }
                }
            }
            kill_process(pidEncoder);
            if(cond == 1){
                ao();
                break;
            }
            //rotacao
            reset_encoder(ENCODER_DIR);
            reset_encoder(ENCODER_ESQ);
            //motor(MOTOR_DIR,-POTENCIA_ROTACAO);
            //motor(MOTOR_ESQ,POTENCIA_ROTACAO);
            pidEncoder = start_process(andaReto(-100,-60,60,100,2));
            //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;
                }
            }
            kill_process(pidEncoder);
            ao();
            if(cond == 1){
                break;
            }
        }
        if(cond == 1) break;
        c++;
        luz = getMediaLightValue(DIF_SENSOR);
    }
    ao();
    
}

int LocalizaFonte360(int escolhido, int tipo){
    int melhor, valorRot;
    int luz;
    int pidEncoder;
    //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);
    pidEncoder = start_process(andaReto(60,100,-100,-60,3));
    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);
            }
        }
    }
    kill_process(pidEncoder);
    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);
    if(tipo == 2 && mseconds() >= 53000L){
        motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
        msleep(4000L);
        off(MOTOR_BLOCO);
        Back(4);
    }
    pidEncoder = start_process(andaReto(-100,-60,60,100,2));
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < valorRot);
    kill_process(pidEncoder);
    ao();
    return 1;
    
}