#use "encoder.ic"

//segue linha
int numColors=4;
persistent int colorValues[40][3];
persistent int UpperBoundEsq, UpperBoundDir, LowerBoundEsq, LowerBoundDir;

//localizacao
persistent int BOUND_LUZ2;
persistent int BOUND_LUZ1;
persistent int BOUND_LUZ1_ENCOSTADO;
persistent int BOUND_LUZ2_ENCOSTADO;

//breakbeam
persistent int BOUND_AMBIENTE_BB;
persistent int BOUND_DIFF_BB;
persistent int Bound_BB;

persistent int estrategia;


void calibracoes(){
    int opcaoIR, numCalib, i;
    char calibs[4][];
    calibs[0] = "polarizacao";
    calibs[1] = "segue linha";
    calibs[2] = "cores";
    calibs[3] = "Break Beam";
    //habilita receptor IR
    //    sony_init(1);
    numCalib = 4;
    
    for(i=0; i < numCalib;i++){
        printf("\n\nDeseja calibrar %s?",calibs[i]);
        while(1){
            //            opcaoIR = ir_data(0);
            /*if(opcaoIR == 61){
                switch(i){
                    case 0:
                      calibraPolarizacao();
                    break;
                    
                    case 1:
                      calibraSegueLinha();
                    break;
                    
                    case 2:
                      calibraCores(COLOR_SENSOR);
                    break;
                    
                    
                }
            }
            else if(opcaoIR == 21){
                break;
            }*/
            if(start_button()){
                while(start_button());
                switch(i){
                    case 0:
                      calibraPolarizacao();
                    break;
                    
                    case 1:
                      calibraSegueLinha();
                    break;                    
                    case 2:
                      calibraCores(COLOR_SENSOR);
                    break;
                    case 3:
                      calibraBreakBeam();
                    break; 
                    
                }
                break;
            }
            else if(stop_button()){
                while(stop_button());
                break;
            }
            sleep(0.1);
        }
    }
    
    printf("\n\nEstrategia?");
    while(1){
        if(start_button()){
            while(start_button());
            estrategia = 1;
            break;
        }
        if(stop_button()){
            while(stop_button());
            estrategia = 2;
            break;
        }
    }
    
    
    
    
    
    
    
    
    //    sony_init(0);
}

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 calibraSegueLinha(){
    int opcaoIR;
    int 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 = getMediaLightValue(LIGHT_SENSOR_DIR);
    LowerBoundEsq = getMediaLightValue(LIGHT_SENSOR_ESQ);
    
    //aumenta ligeiramente o valor do limite inferior
    //LowerBoundDir = LowerBoundDir*1.2;
    //LowerBoundEsq = LowerBoundEsq*1.2;
    
    beep();
    printf("\n\nEsq: %d Dir: %d", 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 = getMediaLightValue(LIGHT_SENSOR_DIR);
    UpperBoundEsq = getMediaLightValue(LIGHT_SENSOR_ESQ);
    
    //reduz ligeiramente o valor do limite superior
    //UpperBoundDir = UpperBoundDir*0.9;
    //UpperBoundEsq = UpperBoundEsq*0.9;
    beep();
    printf("\n\nEsq: %d Dir: %d", UpperBoundEsq, UpperBoundDir);
    sleep(3.0);
    
    //ajusta bounds
    difDir = UpperBoundDir - LowerBoundDir;
    difEsq = UpperBoundEsq - LowerBoundEsq;
    UpperBoundDir -= difDir/5;
    LowerBoundDir += difDir/5;
    UpperBoundEsq -= difEsq/5;
    LowerBoundEsq += difEsq/5;
    
    printf("\n\nCalibracao concluida!");
    sleep(2.0);
    
}

void calibraCores(int port){
    int valor[3];
    int repetir = 0, comObjeto = 0;
    int i,c,opcaoIR=0;
    /*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!");
                    printf("\n\nr:%d g:%d b:%d. NOT ACEPTED",valor[0],valor[1],valor[2]);
                    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);
    }
    
}


void calibraBreakBeam(){
    int bound;
    int out = 0;
    while(1){
        printf("\n\nCubra o LED IR");
        while(1){
            if(start_button()){
                while(start_button());
                break;
            }
        }
        BOUND_AMBIENTE_BB = getMediaLightValue(BREAK_BEAM);
        beep();
        printf("\n\nDeixe o LED descoberto");
        while(1){
            if(start_button()){
                while(start_button());
                break;
            }
        }
        bound = (BOUND_AMBIENTE_BB + getMediaLightValue(BREAK_BEAM))/2;
        BOUND_DIFF_BB = BOUND_AMBIENTE_BB - analog(BREAK_BEAM);
        beep();
        printf("\n\nAmbiente: %d Diff: %d\n", BOUND_AMBIENTE_BB, BOUND_DIFF_BB);
        sleep(5.0);
        printf("Aceitar?\n");
        
        while(1){
            if(start_button()){
                while(start_button());
                out = 1;
                Bound_BB = bound;
                break;
            }else if(stop_button()){
                while(stop_button());
                out = 0;
                break;
            }
        }
        if (out) break;
        
    }
    beep();
}