#define NUM_ULTIMAS_CORES 3
#define NUM_TENTATIVAS_CORES 3
#define NUM_MEDIDAS_BEAM 3

#use "polarizacao.ic"
int ladoInicial;
/*int main(){
    MonitoraBlocos();
    
}*/

void MonitoraBlocos(){
    int corObjeto,i,detected,tempoAlto=0;
    int pidEncoder;
    //habilita saidas digitais
    //poke(0x1009, 0x3c);
    //calibracoes();
    while(1){
        //while(numAtualObjetos > 1) defer();
        while(carregandoObjeto == 1) defer();
        detected = 0;
        //  for(i=0;i<NUM_MEDIDAS_BEAM;i++){
        //ambiente = 
        //if(analog(BREAK_BEAM) - (255-BOUND_AMBIENTE_BB) > 10 ){
        if(getMediaLightValue(BREAK_BEAM) < Bound_BB){
            detected = 1;
            printf("\n\nAchei Bloco %d", analog(BREAK_BEAM));
        }
        //msleep(100L);
        // }
        if(detected == 1 && carregandoObjeto == 0){
            //desabilita motores
            prioridade=1;
            ao();
            while(lockMotor != 0 && lockMotor != 2){
                
                defer();
            }
            lockMotor = 2;
            prioridade=0;
            ao();
            //for(i=0;i<50;i++)hog_processor();
            
            if(mseconds() < 50000L){
                //pega bloco com garra para centralizar
                motor(MOTOR_BLOCO, POTENCIA_BLOCO);
                msleep(4000L);
                off(MOTOR_BLOCO);
                
                
                //le cor do objeto
                corObjeto = LeObjeto();
                tempoAlto = 0;
            }
            
            else{
                //le cor do objeto
                corObjeto = LeObjeto();
                
                if(corObjeto > 0){
                    //pega bloco com garra para centralizar
                    motor(MOTOR_BLOCO, POTENCIA_BLOCO);
                    msleep(4000L);
                    off(MOTOR_BLOCO);
                }
                tempoAlto = 1;
            }
            
            
            if(corObjeto < 0){
                detected = 0;
                //   for(i=0;i<NUM_MEDIDAS_BEAM;i++){
                
                //if(analog(BREAK_BEAM) - (255-BOUND_AMBIENTE_BB) > 10 ){
                if(getMediaLightValue(BREAK_BEAM) < Bound_BB){
                    detected = 1;
                    printf("\n\nEstou com Bloco %d: %d", corObjeto, analog(BREAK_BEAM));
                    //withBlock = 1;
                }
                //msleep(100L);
                // }
                //    for(i=0;i<NUM_TENTATIVAS_CORES && detected == 1;i++){
                corObjeto = LeObjeto();
                //      if(corObjeto >= 0) break;
                //}
            }
            
            if(corObjeto >= 0 && getMediaLightValue(BREAK_BEAM) < Bound_BB){
                //pega bloco com garra para centralizar
                /*motor(MOTOR_BLOCO, POTENCIA_BLOCO);
                msleep(3000L);
                off(MOTOR_BLOCO);*/
                carregandoObjeto = 1;
                if(corObjeto == 0){
                    if(estrategia == 2){
                        if(tempoAlto == 0){
                            if(ladoInicial == 1){
                                Localiza(2,2);
                            }
                            else Localiza(1,2);
                        }
                        
                    }
                    if(tempoAlto == 0){
                        //solta bloco com garra
                        motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
                        msleep(4000L);
                        off(MOTOR_BLOCO);
                        carregandoObjeto = 0;
                        Back(4);
                    }
                }
                
                else{
                    
                    printf("\n\nindex %d",corObjeto);
                    if(estrategia ==  2){
                        Localiza(ladoInicial,1);
                        motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
                        msleep(4000L);
                        off(MOTOR_BLOCO);
                        carregandoObjeto = 0;
                        Back(4);
                    }
                }
                /*if(numAtualObjetos == 0){
                    //solta bloco com garra
                    motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
                    msleep(3000L);
                    off(MOTOR_BLOCO);
                }*/
                
                //numAtualObjetos++;
                
                
            }
            else{
                
                //solta bloco com garra
                if(tempoAlto == 0){
                    motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
                    msleep(4000L);
                    off(MOTOR_BLOCO);
                    carregandoObjeto = 0;
                    
                }
                Back(4);
            }
            
            
            //habilita motores
            lockMotor = 0;
            defer();
        }
        
    }
}



int LeObjeto(){
    int i,dist, menorDist,c,index,count;
    float distZero;
    int comObjeto=0;
    float ind, menorInd,mixInd, minMixInd;
    int valor[3];
    //int ultimasCores[NUM_ULTIMAS_CORES];
    //int ultimoIndex, detectado;
    
    /*for(c=0;c<NUM_ULTIMAS_CORES;c++){
        ultimasCores[c] = -1;
    }*/
    
    
    //for(count=0;count < NUM_ULTIMAS_CORES; count++){
    comObjeto = getRGB(COLOR_SENSOR,valor);
    if(comObjeto == -1) return comObjeto;
    
    //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 % NUM_ULTIMAS_CORES;
    //ultimasCores[ultimoIndex] = index;
    
    //ultimasCores[count] = index;
    
    //}
    
    /*detectado = 1;
    for(c=0;c<NUM_ULTIMAS_CORES;c++){
        if(ultimasCores[c] != index){
            detectado = 0;
        }
    }*/
    
    //if(detectado == 1) return index;
    //else return -2;
    
    return index;
    
    /*
    printf("\n\nerro %f",minMixInd);
    sleep(0.1);
    detectado = 1;
    if(detectado){
        hog_processor();
        //enable(pid);
        objetoDetectado = index;
        //reseta ultimas cores
        //disable(pid);
        defer();
    }*/
    
}