int sensor_rot, sensor_sw, sensor_weiss, finished;
int timer90grd, num_edges=0;
int mode=0;
int turnmode,findtimer,turntime,toggle;

// rot = 49
// sw = 33
// gr = 41
// weiss = 48



task main()
{
  SetSensor (SENSOR_1, SENSOR_LIGHT); // vorn
  SetSensor (SENSOR_2, SENSOR_LIGHT); // Mitte

  SetSensorType (SENSOR_3, SENSOR_TYPE_TOUCH); // entfernungssensoren
  SetSensorMode (SENSOR_3, SENSOR_MODE_RAW);

  SetUserDisplay (mode, 0);

  // Auf Ausgangsposition
  // 1+2 auf sw-Linie, vom roten Quadrat wegschauend
  find_startposition();

  start follow_line;
  start observe_left;
  
}


// Warten zum analysieren für die langsamen Menschen
void pause()
{
  PlaySound (SOUND_CLICK);
  Off(OUT_A+OUT_B);
  Wait(100);
}

// zurücksetzen, bis mitte auf sw
// 90grd rechts bis mitte und vorn auf sw
sub find_startposition()
{
  sensor_rot = SENSOR_1;
  OnRev(OUT_A+OUT_B);
  until (SENSOR_2 < sensor_rot -12);
  // schwarze Linie gefunden - jetzt drehen um 90grd
  sensor_sw = SENSOR_2;
  pause();

  // drehen 90grd
  OnFwd(OUT_A+OUT_B);
  Wait(20);
  ClearTimer(0);
  OnFwd(OUT_A);
  OnRev(OUT_B);
  until (SENSOR_1 < sensor_sw +8);
  timer90grd = FastTimer(0);
  Wait(5); // etwas weiter in die Linie reindrehen
  Off(OUT_A+OUT_B);

  mode = 1;
  PlaySound (SOUND_CLICK);

}

task follow_line()
{
  while (num_edges < 4)
  {
       if (SENSOR_1 < sensor_sw +5) // es reicht nur den vorderen Sensor zu beobachten
       {
         OnFwd (OUT_A+OUT_B);
       }
       else
       {
         // korrigieren nach links
         ClearTimer(0);
         OnFwd(OUT_B);
         Off(OUT_A);
         until (FastTimer(0) > 25 || SENSOR_1 < sensor_sw +5);
         if (SENSOR_1 < sensor_sw +5) Wait(5);
         Off(OUT_A+OUT_B);
         pause();
         
         // vielleicht auch rechts, aber erstmal wieder zurück (doppelte zeit)
         if (SENSOR_1 > sensor_sw +5)
         {
           ClearTimer(0);
           OnFwd(OUT_A);
           Off(OUT_B);
           until (FastTimer(0) > 40 || SENSOR_1 < sensor_sw +5);

           if (SENSOR_1 < sensor_sw +5) Wait(5);
           Off(OUT_A+OUT_B);
           pause();
           
           // Linienende
           if (SENSOR_1 > sensor_sw +5)
           {
             // dann ist die Linie wohl zuende - linksrum 90grd
             PlaySound (SOUND_LOW_BEEP);

             // suche zurückdrehen
             ClearTimer(0);
             OnFwd(OUT_B);
             OnRev(OUT_A);
             until (FastTimer(0) > 12);

             OnFwd (OUT_A+OUT_B);
             until (SENSOR_3 < sensor_sw +10);
             Wait(15); // etwas weiterlaufen

             OnFwd(OUT_B);
             OnRev(OUT_A);
             until (SENSOR_1 < sensor_sw +5);
             Off(OUT_A+OUT_B);
             
             num_edges = num_edges + 1;
             
             pause();
           }

         }

       }
  }
}

// nach links schauen und auf die Dose warten
task observe_left()
{
   Wait(10); // sonst wird schon in der Drehung gesucht(?)
   while (mode == 1)
   {
      if (SENSOR_3 < 300 && Timer(3) < 2)    // 1000
      {
         // Dose links gesehen
         mode = 2;
         stop follow_line;
         start go2can;
         start finetuning;
      }
      if (SENSOR_3 < 300)                    // 1000
      {
         PlaySound (SOUND_CLICK);
         ClearTimer(3);
      }
   }
}

// von der sw Line aus einschwenken
// konstant anpeilen der Dose
// Dose kann auch noch unsichtbar sein für den Front-Sensor
task go2can()
{

    // 90grd einschwenken
    ClearTimer(0);
    OnFwd(OUT_A);
    OnRev(OUT_A);
    until (FastTimer(0) > timer90grd);
    
    // geradeaus
    OnFwd(OUT_A+OUT_B);
    Wait(50);
    
    while (true)
    {
       locate_can();
    }
    
}

// wenn Dose nicht vor der Nase
// links und rechts suchen
// wenn nicht findbar geradeaus weiter
// sonst darauf zu
sub locate_can()
{
     if (SENSOR_3 > 1000)
     {
       // Dose nicht vor der Nase
       turnmode = -1;
       findtimer = -2;
       ClearTimer(3);
       
   while (turnmode < 2 && findtimer < 0)
   {
      if (turnmode == -1 && FastTimer(3) > 100) {turnmode = 1;ClearTimer(3);}  // erst links
      else if (turnmode == 1 && FastTimer(3) > 200) turnmode = 2;  // dann rechts

      if (findtimer == -2 && SENSOR_3 < 900)
      {
         // Anfang Objekt
         findtimer = -1;
         ClearTimer(1);
      }
      else if (findtimer == -1 && SENSOR_3 > 1000)
      {
         // Ende Objekt
         findtimer = FastTimer(1);
         PlaySound (SOUND_DOUBLE_BEEP);
      }

      if (toggle%2 == 0)
      {
         Off(OUT_A+OUT_B);
         Wait(2);
      }
      else
      {
         if (turnmode == -1)
         {
            OnFwd(OUT_B);
            OnRev(OUT_A);
         }
         else
         {
            OnFwd(OUT_A);
            OnRev(OUT_B);
         }
      }
      toggle=toggle+1;
   }

   if (findtimer > 0) turntime = findtimer/2; // Objektmitte
   else turntime = 100; // 90grd
   
   // zurueckdrehen
   ClearTimer(1);
   while (FastTimer(1) < findtimer/2)
   {
          if (toggle%2 == 0)
          {
            Off(OUT_A+OUT_B);
            Wait(2);
          }
          else
          {
            OnFwd(OUT_A);
            OnRev(OUT_B);
          }
          toggle=toggle+1;
   }
   Off(OUT_A+OUT_B);


       
     }
     else
     {
       // Dose steht vor uns
       // geradeaus
       OnFwd(OUT_A+OUT_B);
       Wait(100);
     }
}

// Erkennen, wenn Dose direkt vor uns
// neu anpeilen und draufzu
task finetuning()
{

    // vorderen Sensor aktivieren
    OnFwd(OUT_C);

    while (mode == 2)
    {
       if (SENSOR_3 < 230)
       {
          mode = 3;
          PlaySound (SOUND_UP);

          stop go2can;
          start pickup_can;
       
          locate_can();
       }
    }

}

// nimmt die Dose auf, wenn nah genug
task pickup_can()
{

   while (mode == 3)
   {
     if (SENSOR_3 < 208 && Timer(0) < 2)
     {
         mode == 4;
         Wait(20);
         Off(OUT_A+OUT_B);
         OnRev(OUT_C); // Anheben
         Wait(150);
         OnFwd(OUT_C);

         start go_home;

     }
     if (SENSOR_3 < 208)
     {
       PlaySound (SOUND_CLICK);
       ClearTimer(0);
     }
   }
   
}

task go_home()
{
   if (num_edges == 0)
   {
      turn_left();
      until_line();
      find_line_left();
      start follow_line;
   }
   else
   if (num_edges == 1)
   {
      until_line();
      find_line_left();
      start follow_line;
   }
   else
   if (num_edges == 2)
   {
      until_line();
      find_line_right();
      start follow_line;
   }
   else
   if (num_edges == 3)
   {
      turn_right();
      until_line();
      find_line_right();
      start follow_line;
   }


}

sub until_line()
{
   OnFwd(OUT_A+OUT_B);
   until (SENSOR_2 < sensor_sw +5);
}

sub turn_left()
{
   // links 90
   ClearTimer(0);
   OnFwd(OUT_B);
   OnRev(OUT_A);
   until (FastTimer(0) > timer90grd);
}

sub turn_right()
{
   // recht 90
   ClearTimer(0);
   OnFwd(OUT_A);
   OnRev(OUT_B);
   until (FastTimer(0) > timer90grd);
}

sub find_line_left()
{
   OnFwd(OUT_B);
   OnRev(OUT_A);
   until (SENSOR_1 < sensor_sw +5);
}

sub find_line_right()
{
   OnFwd(OUT_A);
   OnRev(OUT_B);
   until (SENSOR_1 < sensor_sw +5);
}