#use "seguelinha.ic"

int main(){
    int pidInicial, pidBloco;
    int pidSensors;
    int InitialBoundEsq, InitialBoundDir;
    //habilita saidas digitais
    poke(0x1009, 0x3c);
    //seta leds apagados
    poke(0x1008, 0x00);
    enable_encoder(ENCODER_DIR);    
    enable_encoder(ENCODER_ESQ);
    calibracoes();
    printf("\n\nStart to Begin!");
    start_press();
    //estabelece bounds para largada
    InitialBoundEsq = getMediaLightValue(LIGHT_SENSOR_ESQ);
    InitialBoundDir = getMediaLightValue(LIGHT_SENSOR_DIR);
    InitialBoundEsq += (250 - InitialBoundEsq) / 2;
    InitialBoundDir += (250 - InitialBoundDir) / 2;
    //espera largada
    printf("Esperando largada!\n");
    while(getMediaLightValue(LIGHT_SENSOR_ESQ) < InitialBoundEsq && getMediaLightValue(LIGHT_SENSOR_DIR) < InitialBoundDir);
    printf("Comecou\n");
    reset_system_time();
    
    pidInicial = start_process(cicloPrincipal(),5,350);
    pidBloco = start_process(MonitoraBlocos(),5);
    pidSensors = start_process(MonitorSensors(),5,50);
    //start_process() ...
    
    while(mseconds() < 60000L);
    //   while(mseconds() < 600000L);   
    printf("\n\nFinish");
    kill_process(pidInicial);
    kill_process(pidBloco);
    kill_process(pidSensors);
    ao();
    //kill_process() ...
    
    
    
}

void cicloPrincipal(){
    //int pidSegueLinha, pidSensors;
    cicloInicial();
    FollowLine();
    
    //pidSegueLinha = start_process(FollowLine(),5);
    
    
}

void cicloInicial(){
    determinaInicial();
    Rotate(DIR,ROT_180);
    //Rotate90(DIR);
    motor(MOTOR_DIR,POTENCIA_MOTOR_DIR);
    motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
    msleep(2000L);
    //sobeRampa();
    //nao precisa implementar para previa
    //pegaAgua();
}


void determinaInicial(){
    int melhorBaixo, melhorAlto, valorRotBaixo, valorRotAlto, valorRot;
    int luz;
    int pidEncoder;
    //gira p/ a posicao que maximiza ou minimiza Light
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    melhorBaixo = getMediaLightValue(DIF_SENSOR);
    melhorAlto = melhorBaixo;
    valorRotBaixo = 0;
    valorRotAlto = 0;
    //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_360){
        luz = getMediaLightValue(DIF_SENSOR);
        if(luz > melhorAlto){
            melhorAlto = luz;
            valorRotAlto = read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ);
        }
        else if(luz < melhorBaixo){
            melhorBaixo = luz;
            valorRotBaixo = read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ);
        }
    }
    kill_process(pidEncoder);
    ao();
    //descobre e seta o lado do campo
    if(125 - melhorBaixo > melhorAlto - 125){
        ladoInicial = 2;
    }
    else{
        ladoInicial = 1;
    }
    
    printf("Lado inicial: %d\n", ladoInicial);
    //vira de frente para a luz
    valorRot = read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ);
    if(ladoInicial == 1){
        //valorRot -= valorRotBaixo;
        valorRot = valorRotBaixo;
    }
    else{
        //valorRot -= valorRotAlto;
        valorRot = valorRotAlto;
    }
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    //motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
    //motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
    
    //Rotate(ESQ,valorRot);
    Rotate(DIR,valorRot);
    
    //while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < valorRot);
    //ao();
}

void sobeRampa(){
    int pidEncoder;
    //condEncoder = 1;
    printf("Estou subindo a rampa lalala\n");
    //pidEncoder = start_process(andaReto(-100,-60,-1));
    //detectar subida de rampa a partir de counts encoder/seg
    //ou usar algum sensor de toque (embaixo do carro por ex) para isso
    msleep(5000L);
    
    // kill_process(pidEncoder);
    
}

void pegaAgua(){
    //gira 180 graus e pega agua
}


//importar movimento aleatorio e implementar pegar bloquinhos