// Variables pour capteurs
byte dir, s1,s2,s3,s4,s5;
byte dist;
bool result;
//Variables d'état du programme et du jeu
int balle = 0, capture = 0, arret = 0, demitour = 0;

/*
task affiche_s()
{
  SetSensorLowspeed(IN_2);
  while(true)
  {
  result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
  TextOut(0, LCD_LINE1,"Valeurs des s");
  TextOut(0,LCD_LINE2,"    ");
  NumOut(0,LCD_LINE2, s1);
  TextOut(0,LCD_LINE3,"    ");
  NumOut(0,LCD_LINE3, s2);
  TextOut(0,LCD_LINE4,"    ");
  NumOut(0,LCD_LINE4, s3);
  TextOut(0,LCD_LINE5,"    ");
  NumOut(0,LCD_LINE5, s4);
  TextOut(0,LCD_LINE6,"    ");
  NumOut(0,LCD_LINE6, s5);
  }
}*/



task principal()
{
  SetSensorLowspeed(IN_2);
  SetHTIRSeeker2Mode(IN_2, HTIR2_MODE_600);
  SetSensorUltrasonic(IN_3);
  
  result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
  capture = 0;
 
  while (1)
  {    
    while(capture == 0)
    {
      result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
      if (arret == 1 || demitour == 1) continue;
      
      
      if (s1 > 0 || s2 > 0)
      {
        OnFwd(OUT_A,-100);
        OnFwd(OUT_B,100);
      }
      
      if (s4 > 0 || s5 > 0)
      {
        OnFwd(OUT_A,100);
        OnFwd(OUT_B,-100);
      }

      if (s3 > 0)
      {
        OnFwd(OUT_AB,80);

        dist = SensorUS(IN_3);
        result = ReadSensorHTIRSeeker2AC(IN_2,dir,s1,s2,s3,s4,s5);
    
        if (s3 >= 150)  //Indiquer que c'est bien la balle qui est juste devant
          balle = 1;

  
        if ((dist <= 8) && (balle == 1))
        {
          RotateMotor(OUT_C, 70, 90);
          Off(OUT_AB);
          Wait(1000);
          dist = SensorUS(IN_3);
          if (dist > 8) 
           {
           RotateMotor(OUT_C, 70, -90);
           continue;
           }
           
          else
            capture = 1;       
        }
      }
    }
// Tir
     // demi_tour
            OnFwd(OUT_A,50);
            OnFwd(OUT_B,-50);
            demitour = 1;
            Wait(3000);
            Off(OUT_AB);
            demitour = 0;
        // lancer de balle             
            RotateMotor(OUT_C, 70, -90);  
            balle = 0;
            Wait(1000);
            OnFwd(OUT_AB,80);
            capture = 0;
            
    
  }
 }
  
 

task main()
{
 //StartTask(demi_tour_ligne_rouge); 
 StartTask(principal);
// StartTask(arret_obstacle); 

  while (true)
  Wait(1000);
}