byte dir, s1,s2,s3,s4,s5, stemp;
bool result;
int i = 0;
int is3, istemp; // int qui vont contenir les valeurs de s3 et stemp, puisque les int sont plus faciles à manipuler


task affichage()
{
SetSensorLowspeed(IN_2);
  while(1)
  {
    result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
    TextOut(0, LCD_LINE2,"     ");
    NumOut(0,LCD_LINE2, stemp);
    TextOut(0, LCD_LINE3,"     ");
    NumOut(0,LCD_LINE3, s3);
    is3 = s3;
    istemp = stemp;
    Wait(250);
  }
}

sub tir()
{
  RotateMotor(OUT_C, 100, -80);
  Wait(250);
  RotateMotor(OUT_C, 100, 80);
}


sub cherche_but()
{
  while(1)
  {
    stemp = 0; 
    i = 0;

    SetSensorLowspeed(IN_2);
    SetHTIRSeeker2Mode(IN_2, HTIR2_MODE_1200);
    result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
     
    while (s3 <= 50) // On cherche a se diriger face au premier but qu'on rencontrera
    {
      TextOut(0, LCD_LINE7,"                     ");
      TextOut(0, LCD_LINE7,"Se mettre face au but");
      Wait(1000);
      
      result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
      OnFwd(OUT_A, 35);
      OnFwd(OUT_B, -35);
    }
    Off(OUT_AB);
    
    TextOut(0, LCD_LINE7,"                 ");
    TextOut(0, LCD_LINE7,"Face au but");
    Wait(1000);
    
    // Le robot est face à un but
    result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
    stemp = s3; //Stockage de s3 du moment dans stemp
  
    while(1)
    {
      TextOut(0, LCD_LINE6,"                    ");
      TextOut(0, LCD_LINE6,"Boucle 1");
      Wait(1000);

      result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
    
       if (abs(is3-istemp) > 50) // différence de + de 50 = changement significatif
      {
        TextOut(0, LCD_LINE7,"                   ");
        TextOut(0, LCD_LINE7,"Premier changement");
        break;
      }
    }
  
    result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3 ,s4,s5);
    stemp = s3; //Stockage de s3 du moment dans stemp
  
    while(1) //Considérons que nous avons un but qui fait des séquences 4 sec - 4 sec
    {
      TextOut(0, LCD_LINE6,"                      ");
      TextOut(0, LCD_LINE6,"Boucle 2");
      Wait(1000);
      
      result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
      TextOut(0, LCD_LINE8,"     ");
      NumOut(0,LCD_LINE8, i); 
      
      if (abs(is3-istemp) > 50) // différence de + de 50 = changement significatif
        {
          TextOut(0, LCD_LINE7,"                    ");
          TextOut(0, LCD_LINE7,"Deuxieme changement");
          Wait(1000); 
          break;
        }
      else
      {
        i++;
        TextOut(0, LCD_LINE7,"                   ");
        TextOut(0, LCD_LINE7,"Incrémentation de i");
      }
  }
  
  if (i == 4)
    {
      tir();
      break;
    }
  else // On va chercher l'autre but
  {
    result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
    OnFwd(OUT_A, 35);
    OnFwd(OUT_B, -35);
    TextOut(0, LCD_LINE7,"                           ");
    TextOut(0, LCD_LINE7,"Demi-tour");
    Wait(2000);
  }
}
}




task main()
{
StartTask(affichage);
cherche_but();

}