#define EYE SENSOR_1
#define LEFT      OUT_C
#define RIGHT     OUT_A
#define BUMPER SENSOR_2  // bump
 
#define NORMAL_SPEED 6
#define TURN_SPEED 2
#define DELAY     20
 
#define CUTOFF    55
#define STOP      48
 
int direction, time, eye;
 
#define INITIAL_TIME 2
// timing
#define BUMP_BACK_TIME  60  // 0.6 seconds  bump
#define BUMP_SPIN_TIME  20  // 0.2 seconds  bump
 
task main()
{
      SetSensor(EYE, SENSOR_LIGHT);
      
      direction = 1;
      time = INITIAL_TIME;
      start follow;
      stop avoid_Obstacle;
      stop external_Control;
      while (eye < STOP)
      {
         if (Message() != 0)
       {
           stop follow;
           stop avoid_Obstacle;
           start external_Control;
           Wait (50);
           start follow;
       }
         if (BUMPER==0)
       {
           stop follow;
           start avoid_Obstacle;
       }
      }
      stop follow;      
      Off(RIGHT+LEFT);
      PlaySound(5);
}
 
 
task follow()
{
      SetPower(LEFT+RIGHT, NORMAL_SPEED);
      OnFwd(LEFT+RIGHT);
      while(true)
      {
            eye = EYE;
            if (eye <= CUTOFF) continue;
 
            ClearTimer(0);
            if (direction == 1)
            {
                  SetPower(LEFT+RIGHT, TURN_SPEED);
                  Fwd(RIGHT);
                  Rev(LEFT);
            }
            else
            {
                  SetPower(LEFT+RIGHT, TURN_SPEED);
                  Fwd(LEFT);
                  Rev(RIGHT);
            }
                  
            while(true)
            {
                  if (EYE < CUTOFF)
                  {
                        time = INITIAL_TIME;
                        break;
                  }
                        
                  if (Timer(0) > time)
                  {
                        direction *= -1;
                        time *= 2;
                        break;
                  }
            }
                  
            SetPower(RIGHT+LEFT, NORMAL_SPEED);
            Fwd(RIGHT+LEFT);
      }
}
 
 
task avoid_Obstacle()
{           
       // back up a bit       
               OnRev(LEFT+RIGHT);
               Wait(BUMP_BACK_TIME);
            
               // spin a bit
               Fwd(LEFT);
               Wait(BUMP_SPIN_TIME);
            
               // resume
               Fwd(RIGHT);
               Wait(BUMP_SPIN_TIME);
       start follow;          
}
 
task external_Control()
{
         if (Message() == 1) {OnFwd(OUT_A+OUT_C);}
         if (Message() == 2) {OnRev(OUT_A+OUT_C);}
         if (Message() == 3) {Off(OUT_A+OUT_C);}
         ClearMessage();
}
 
 
#define SENSOR_LUZ            SENSOR_2
#define ESQUERDA    OUT_C
#define DIREITA         OUT_A
#define ADIANTE (OUT_REV)
#define REVERSO (OUT_FWD)
#define VELOCIDADE_NORMAL 10
#define VELOCIDADE_REDUZIDA 1
// TEMPOS EM 1/10 S
#define TEMPO_PAUSA (1)
#define TEMPO_EXTRA (1)
#define TEMPO_SEGUINDO (5)
#define TEMPO_IDEAL (3)
// ÂNGULO SÃO TEMPOS DE RETORNO
#define ANGULO_INCIAL (3)
#define ANGULO_PASSO (2)
// NIVEIS DE LUZ EM PERCENTAGEM
#define PONTO_INCIAL       (43)
#define LIMITE_BORDA        (24)
int direcao, angulo_volta, sensor_luz, relogio;
task main()
{
    SetSensor(SENSOR_LUZ, SENSOR_LIGHT);
 
    direcao = 1;
    angulo_volta = ANGULO_INCIAL;
    sensor_luz = SENSOR_LUZ;
    while (true) {
        procurar();  //até encontrar a linha
        seguir();  // até perder novamente a linha
    }
}
// fica parado por uns instantes
void fica_parado()
{
    Off(ESQUERDA+DIREITA);
        // Espera medida em centésimos de seg
#if TEMPO_PAUSA > 0
   Wait(TEMPO_PAUSA * 10);
#endif
}
void esperando_a_linha(const int &timeout)
{
    ClearTimer(0);
    relogio = 0;
    while (sensor_luz >= PONTO_INCIAL && relogio < timeout) {
        sensor_luz = SENSOR_LUZ;
        relogio = Timer(0);
    }
}
 
// procurar a linha até o sensor de luz a enchergar
sub procurar()
{
    int tempo_volta;
    
    SetPower(ESQUERDA + DIREITA, VELOCIDADE_REDUZIDA);
        // tentando ir de um lado ao outro
    tempo_volta = angulo_volta;
    while (true) {
        if (direcao == 1) {
            SetDirection(DIREITA, ADIANTE);
            SetDirection(ESQUERDA, REVERSO);
        } else {
            SetDirection(ESQUERDA, ADIANTE);
            SetDirection(DIREITA, REVERSO);
        }
        On(ESQUERDA + DIREITA);
        esperando_a_linha(tempo_volta);
        if (sensor_luz < PONTO_INCIAL) {
            return;
        }
            // voltando para trás
        SetDirection(ESQUERDA + DIREITA, REVERSO);
        On(ESQUERDA + DIREITA);
        esperando_a_linha(angulo_volta);
        fica_parado();
        if (sensor_luz < PONTO_INCIAL) {
            return;
        }
            // não esta achando desta maneira, tentar de outra.
 
        direcao *= -1;
        tempo_volta = angulo_volta; // voltando ao ponto de parada
        angulo_volta += ANGULO_PASSO;
        tempo_volta += angulo_volta;
    }
}
// Achou a linha e segue atá perder.
sub seguir()
{
    int successo;
  
    successo = 0;
    SetPower(ESQUERDA + DIREITA, VELOCIDADE_NORMAL);
    SetDirection(ESQUERDA + DIREITA, ADIANTE);
    while (true) {
            // mudando a direção até encontrar a linha
        if (direcao == 1) {
            Off(ESQUERDA);
            On(DIREITA);
        } else {     
            Off(DIREITA);
            On(ESQUERDA);
        }
        esperando_a_linha(TEMPO_SEGUINDO);
        if (sensor_luz >= PONTO_INCIAL) {
            return;
        }
        if (sensor_luz < LIMITE_BORDA) {
                // abortar
            Off(ESQUERDA + DIREITA);
            StopAllTasks();
                // para toda as tarefas
        }
            // encontrou a linha
        if (relogio < TEMPO_IDEAL) {
            successo++;
        } else {
            successo = 0;
        }
        if (successo > 3) {
                // encontrou alinha
            angulo_volta = ANGULO_INCIAL;
        }
            // ESPERANDO UM TEMPO EXTRA E MUDANDO A DIREÇÃO
            // espera medida em centésimo de seg
        Wait(TEMPO_EXTRA * 10);
        direcao *= -1;
            // sensor fora da linha
        sensor_luz = SENSOR_LUZ;
    }
}