#use "bloco.ic"
int estado, ultimoEstado = -1;


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

int MonitorSensors(){
    int valueDir, valueEsq;
    
    ultimoEstado = estado;
    
    while(1){
        valueDir = getMediaLightValue(LIGHT_SENSOR_DIR);
        valueEsq = getMediaLightValue(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;
        }
        
        defer();
        
    }
    
}

/*
int FollowLine(){
    int valorDir, valorEsq;
    int ultimo0 = 0;
    int i,c;
    int pidEncoder;
    long tempoAux;
    float secAux;
    float tempoA;
    
    while(1){
        
        //acoes por deteccao de blocos
        //if(ficarParado == 0){
        
        if(digital(TOQUE_ESQ) || digital(TOQUE_DIR) || digital(TOQUE_FRENTE)){
            beep();
            
            while(lockMotor != 0 && lockMotor != 1) defer();
            lockMotor = 1;
            
            motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
            motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
            tempoAux = mseconds();
            while(tempoAux + 500L < mseconds()){
                defer();
            }
            //sleep(0.5);
            ao();
            reset_encoder(ENCODER_DIR);
            reset_encoder(ENCODER_ESQ);
            //motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
            //motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
            pidEncoder = start_process(andaReto(-100,-60,60,100,2));
            while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_180+3 && estado != 0){
                if(prioridade == 1){
                    kill_process(pidEncoder);
                    ao();
                    lockMotor = 2;
                    defer();
                    while(lockMotor!= 0){
                        defer();
                    }
                    lockMotor = 1;
                    pidEncoder = start_process(andaReto(-100,-60,60,100,2));
                }
            }
            while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_180+3 && estado == 0){
                if(prioridade == 1){
                    kill_process(pidEncoder);
                    ao();
                    lockMotor = 2;
                    defer();
                    while(lockMotor!= 0){
                        defer();
                    }
                    lockMotor = 1;
                    pidEncoder = start_process(andaReto(-100,-60,60,100,2));
                }
            }
            kill_process(pidEncoder);
            ao();
            lockMotor = 0;
        }
        
        if(carregandoObjeto == 1){
            // if(getMediaLightValue(BREAK_BEAM) > Bound_BB){
            //     carregandoObjeto = 0;
            //     printf("\n\ncancelado");
            //  }
            //  else{
            printf("\n\nlocalizando");
            Localiza(ladoInicial);
            printf("\n\nlocalizou");
            /*Rotate(DIR,ROT_90);
            motor(MOTOR_DIR,POTENCIA_MOTOR_DIR);
            motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
            tempoAux = mseconds();
            while(!digital(TOQUE_ESQ) && !digital(TOQUE_DIR) && !digital(TOQUE_FRENTE) && (mseconds() < tempoAux + (long)5000));
            ao();*/

//solta bloco com garra
/*          motor(MOTOR_BLOCO, -POTENCIA_BLOCO);
            msleep(3500L);
            off(MOTOR_BLOCO);
            carregandoObjeto = 0;
            motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
            motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
            msleep(1000L);
            ao();
            Rotate(DIR,ROT_180);
            motor(MOTOR_DIR,POTENCIA_MOTOR_DIR);
            motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
            msleep(1000L);
            ao();
            //   }
            
        }
        
        else{
            
            //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_MOTOR_DIR);
                //motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
                pidEncoder = start_process(andaReto(60,100,-100,-60,3));
                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90 && estado == 0);
                kill_process(pidEncoder);
                //ao();
                if(estado == 0){
                    beep();
                    //rotacao 360 graus
                    reset_encoder(ENCODER_DIR);
                    reset_encoder(ENCODER_ESQ);
                    pidEncoder = start_process(andaReto(-100,-60,60,100,2));
                    //motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
                    //motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
                    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_360 && estado == 0);
                    kill_process(pidEncoder);
                    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();
                                tempoA = seconds();
                                while(estado == 0 && seconds() < tempoA+(float)c*1.5){
                                    if(digital(TOQUE_ESQ) || digital(TOQUE_DIR) || 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_MOTOR_DIR);
                                //motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
                                pidEncoder = start_process(andaReto(-100,-60,60,100,2));
                                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90 && estado == 0);
                                kill_process(pidEncoder);
                                ao();
                                if(estado != 0){
                                    break;
                                }
                            }
                            c++;
                        }
                        printf("\n\nLinha achada!");
                        beep();
                    }
                }
                ultimo0 = 1;
                
            }
            if(ultimo0 != 1){
                while(lockMotor != 0 && lockMotor != 1) defer();
                lockMotor = 1;
                
                if(prioridade == 1){
                    ao();
                    lockMotor = 2;
                    defer();
                    while(lockMotor!= 0 && lockMotor != 1){
                        defer();
                    }
                    lockMotor = 1;
                }
                
                motor(MOTOR_ESQ,valorEsq);
                motor(MOTOR_DIR,valorDir);
            }
            ultimo0 = 0;
            
            
        }
        
        
    }
    
    // else{
    //   off(MOTOR_ESQ);
    //   off(MOTOR_DIR);
    //   defer();
    //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();
// }
//}


//}

void RotateUntilNotLine(int sentido, int rotMax){
    int pid;
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    if(sentido == DIR){
        pid = start_process(andaReto(-100,-60,60,100,2));
    }
    else{
        pid = start_process(andaReto(60,100,-100,-60,3));
    }
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < rotMax && estado != 0) defer();
    kill_process(pid);
    ao();
}

void RotateUntilLine(int sentido, int rotMax){
    int pid;
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    if(sentido == DIR){
        pid = start_process(andaReto(-100,-60,60,100,2));
    }
    else{
        pid = start_process(andaReto(60,100,-100,-60,3));
    }
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < rotMax && estado == 0) defer();
    kill_process(pid);
    ao();
}


void FollowLineUntilLost(long tempoMin, int sentidoCorrecao){
    int valorDir, valorEsq;
    int countEstate3=0;
    int ultimo0 = 0;
    int i,c;
    int pid, pStarted=0, pid2;
    long tempoAux = mseconds()+tempoMin;
    
    while(1){
        //maquina de estados
        if(prioridade == 1){
            //kill_process(pid);
            ao();
            lockMotor = 2;
            defer();
            while(lockMotor!= 0){
                defer();
            }
            lockMotor = 1;
            //pid = start_process(andaReto(60,100,60,100,1));
        }
        if(digital(TOQUE_DIR) || digital(TOQUE_ESQ) || digital(TOQUE_FRENTE)){
            reset_encoder(ENCODER_DIR);
            reset_encoder(ENCODER_ESQ);
            pid = start_process(andaReto(-100,-60,-100,-60,-1));
            while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < 10 && estado == 2) defer();
            kill_process(pid);
            ao(); 
        }
        if(digital(TOQUE_DIR) && !digital(TOQUE_ESQ)){
            //Back();
            if(estado == 2){
                reset_encoder(ENCODER_DIR);
                reset_encoder(ENCODER_ESQ);
                pid = start_process(andaReto(60,100,-100,-60,3));
                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90 && estado == 2) defer();
                kill_process(pid);
                ao();
            }
            else{
                RotateUntilNotLine(ESQ,ROT_45);
            }
        }
        if(digital(TOQUE_ESQ) && !digital(TOQUE_DIR)){
            //Back();
            if(estado == 2){
                reset_encoder(ENCODER_DIR);
                reset_encoder(ENCODER_ESQ);
                pid = start_process(andaReto(-100,-60,60,100,2));
                while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < ROT_90 && estado == 2) defer();
                kill_process(pid);
                ao();
            }
            else{
                RotateUntilNotLine(DIR,ROT_45);
            }
        }
        
        switch(estado){
            
            case 3:
             /* if(pStarted == 0){
                pid2 = start_process(andaReto(60,100,60,100,1));
                pStarted = 1;
            }*/
            valorDir = POTENCIA_MOTOR_DIR;
            valorEsq = POTENCIA_MOTOR_ESQ;
            break;
            
            case 2:
             /* if(pStarted){
                kill_process(pid2);
                pStarted = 0;
            }*/
            valorDir = POTENCIA_MOTOR_DIR;
            valorEsq = 0;
            break;
            
            case 1:
             /* if(pStarted){
                kill_process(pid2);
                pStarted = 0;
            }*/
            valorDir = 0;
            valorEsq = POTENCIA_MOTOR_ESQ;
            break;
            
            default:
            /*  if(pStarted){
                kill_process(pid2);
                pStarted = 0;
            }*/
            if(tempoAux <= mseconds()){
                ao();
                return;
            }
            else{
                RotateUntilLine(sentidoCorrecao,ROT_180);
                if(estado == 0){
                    if(sentidoCorrecao == DIR){
                        RotateUntilLine(ESQ,ROT_360+ROT_90);
                    }
                    else{
                        RotateUntilLine(DIR,ROT_360+ROT_90);
                    }
                }
                
            }
            
        }
        //if(!pStarted){
            motor(MOTOR_ESQ,valorEsq);
            motor(MOTOR_DIR,valorDir);
        //}
        
        
    }
    
    
}