// This source code is for Not Quite C (.nqc)

sub initsensors() // de sensoren moeten eerst worden geinitialiseerd
{
// SENSOR_1 is de lichtsensor, te meten als percentage.
// Dit kun je uiteraard veranderen, mocht je dat nodig vinden.
       SetSensor(SENSOR_1, SENSOR_LIGHT);
       SetSensorMode(SENSOR_1, SENSOR_MODE_PERCENT);

// SENSOR_2 en _3 zijn touchsensors, die we in boolean mode zetten.
// In bijvoorbeeld SENSOR_MODE_RAW zou je ook de mate van touch kunnen uitlezen.
       SetSensor(SENSOR_2, SENSOR_TOUCH);
       SetSensorMode(SENSOR_2, SENSOR_MODE_BOOL);

       SetSensor(SENSOR_3, SENSOR_TOUCH);
       SetSensorMode(SENSOR_3, SENSOR_MODE_BOOL);
}

// Initialisatie
#define wanderSound PlaySound(0);

int grenswaarde=44;                //Waarde tussen licht en donker in (cut-off)
int voorkeursrichting=0;           //Richting die als eerste bekeken wordt
int powerL;                        //Power bij links-draaien
int powerR;                        //Power bij rechts-draaien
int powerFW;                       //Power bij vooruit rijden
int powerBW;                       //Power bij achteruit rijden
int tijd;                          //Lengte van rijtijd in 10de seconden

/*
Relatieve lichtmeting, is verworpen voor absolute waarde (grenswaarde)

int laatstGezien;                  //Laatst waargenomen waarde van lichtsensor

task lichtgeheugen()
{
       laatstGezien=SENSOR_1;
}
*/

sub gaVooruit()                    //Ga vooruit met aangegeven power en tijd
{
       SetPower(OUT_C+OUT_A,powerFW);
       OnFwd(OUT_A);
       OnRev(OUT_C);
       Wait(tijd*40);
}

sub gaAchteruit()                  //Ga achteruit met aangegeven power en tijd
{
       SetPower(OUT_C+OUT_A,powerBW);
       OnFwd(OUT_C);
       OnRev(OUT_A);
       Wait(tijd*40);
}


sub draaiLinks()                   //Ga links met aangegeven power en tijd
{
       SetPower(OUT_C+OUT_A,powerL);
       OnFwd(OUT_C+OUT_A);
       Wait(tijd*5);
}

sub draaiRechts()                  //Ga rechts met aangegeven power en tijd
{
       SetPower(OUT_C+OUT_A,powerR);
       OnRev(OUT_C+OUT_A);
       Wait(tijd*5);
}

task wander()                      //Loop rond
{
       wanderSound;                       //HerkenningsMelodietje voor wander
       stop followPath;                   //Onderbreek followPath
       while(true)
       {
              powerFW=7;
              int richting = Random(1);   //Bepaal richting random
              tijd = Random(12)+12;       //Bepaal draai- en rijtijd random
              powerL = Random(3)+4;       //Bepaal draai- en rijsnelheid random
              powerR = Random(3)+4;
              gaVooruit();                //Ga vooruit
              if (richting==1)            //Draai de bepaalde richting
                     draaiLinks();
              else
                     draaiRechts();
       }
}

task followPath()                  //Volg pad
{
       PlaySound(0);                      //HerkenningsMelodietje voor followPath
       stop wander;                       //Onderbreek wander
       while (true)
       {
              stop followreleaser;        //onderbreek followReleaser
              SetPower(OUT_A+OUT_C, 7);   //Zet power op vol
              OnFwd(OUT_A);               //Standaard : rij vooruit
              OnRev(OUT_C);

              int draaiduur;
              int autodraai=20;
              int kleinedraai=25;
              int mediumdraai=60;
              int grotedraai=250;
              int eersteSchatting=0;
              int tweedeSchatting=0;
      	       int doordraaitijd=10;

              while (SENSOR_1 > grenswaarde)     //Als op wit
              {

                     Off(OUT_A+OUT_C);                         //Stop met rijden
                     int i;
                     int j;
		       if (voorkeursrichting==0)               //
		       {
                            for (i=0;i<3;i++)
                            {
                                   if (i==0)
                                          draaiduur=kleinedraai;
                                   if (i==1)
                                          draaiduur=mediumdraai;
                                   if (i==2)
                                          draaiduur=grotedraai;
                                   OnRev(OUT_A+OUT_C);
                                   for (j=0;j<draaiduur;j++)
	                            {
              	                     eersteSchatting=j;
       	                            if (SENSOR_1 < grenswaarde)
       	                            {
                                                 break;
                	                     }
                	                    Wait(1);
       	                     }
       	                     if (eersteSchatting<autodraai)
       	                     {
                                          Wait(doordraaitijd);
                                          break;
                                   }
       	                     OnFwd(OUT_A+OUT_C);
       	                     for (j=0;j<eersteSchatting+1;j++)
       	                     {
                                          Wait(1);
                                   }
                                   for (j=0;j<eersteSchatting+2;j++)
                                   {
                                          tweedeSchatting=j;
                                          if (SENSOR_1 < grenswaarde)
                                          {
                                                 Wait(doordraaitijd);
                                                 break;
                                          }
                                          Wait(1);
                                    }
                                    if (tweedeSchatting<autodraai)
       	                      {
                                          voorkeursrichting=1;
                                          Wait(doordraaitijd);
                                          break;
                                    }
       	                      if (tweedeSchatting<eersteSchatting)
                                    {
                                          voorkeursrichting=1;
                                          break;
                                    }
                                    if (eersteSchatting==(draaiduur-1)&&i==2)
                                    {
                                          start wander;
                                          break;
                                    }
                                    OnRev(OUT_A+OUT_C);
                                    for (j=0;j<tweedeSchatting+1;j++)
                                    {
                                          Wait(1);
                                    }
                                    if ((i==0||i==1)&&eersteSchatting<draaiduur-1)
                                    {
                                          Wait(eersteSchatting);
                                          Wait(doordraaitijd);
                                          break;
                                    }
                            }

                   }
		     if (voorkeursrichting==1)
		     {
                            for (i=0;i<3;i++)
                            {
                                   if (i==0)
                                          draaiduur=kleinedraai;
                                   if (i==1)
                                          draaiduur=mediumdraai;
                                   if (i==2)
                                          draaiduur=grotedraai;
                                   OnFwd(OUT_A+OUT_C);
                                   for (j=0;j<draaiduur;j++)
	                            {
              	                     eersteSchatting=j;
       	                            if (SENSOR_1 < grenswaarde)
       	                            {
                                                 break;
                	                     }
                	                    Wait(1);
       	                     }
       	                     if (eersteSchatting<autodraai)
       	                     {
                                          Wait(doordraaitijd);
                                          break;
                                   }
       	                     OnRev(OUT_A+OUT_C);
       	                     for (j=0;j<eersteSchatting+1;j++)
       	                     {
                                          Wait(1);
                                   }
                                   for (j=0;j<eersteSchatting+2;j++)
                                   {
                                          tweedeSchatting=j;
                                          if (SENSOR_1 < grenswaarde)
                                          {
                                                 Wait(doordraaitijd);
                                                 break;
                                          }
                                          Wait(1);
                                    }
                                    if (tweedeSchatting<autodraai)
       	                      {
                                          voorkeursrichting=0;
                                          Wait(doordraaitijd);
                                          break;
                                    }
       	                      if (tweedeSchatting<eersteSchatting)
                                    {
                                          voorkeursrichting=0;
                                          break;
                                    }
                                    if (eersteSchatting==(draaiduur-1)&&i==2)
                                    {
                                          start wander;
                                          break;
                                    }
                                    OnFwd(OUT_A+OUT_C);
                                    for (j=0;j<tweedeSchatting+1;j++)
                                    {
                                          Wait(1);
                                    }
                                    if ((i==0||i==1)&&eersteSchatting<draaiduur-1)
                                    {
                                          Wait(eersteSchatting);
                                          Wait(doordraaitijd);
                                          break;
                                    }
                            }

                   }
              }
       }
}

task avoid()
{
       while (true)
       {
              //Als 1 of beide bumpers worden ingedrukt
              if (SENSOR_2||SENSOR_3)
              {
                     PlaySound(5);
                     //Laat hem even uitrijden
                     Wait(10);
                     Float(OUT_A+OUT_C);
                     if (SENSOR_2&&SENSOR_3)//Als beide bumpers ingedrukt
                     {
                            if (SENSOR_1>grenswaarde)//Als op wit
                            {
                                   //Onderbreek wander
                                   stop wander;
                                   SetPower(OUT_A+OUT_C,4);
                                   //Draai 3 keer om as (2e keer langer)
                                   //Hierdoor komt hij van de muur af
                                   OnFwd(OUT_C);
                                   Wait(50);
                                   OnFwd(OUT_A);
                                   Wait(500);
                                   OnRev(OUT_C);
                                   Wait(50);
                                   //Hervat wander
                                   start wander;
                            }
                      }
                      else if (SENSOR_2)//Als linker-bumper ingedrukt
                      {
                            PlaySound(1);
                            if (SENSOR_1<grenswaarde)//Obstakel tijdens followPath
                            {
                                   stop followPath;     //Onderbreek followPath
                                   powerL=4;            //Stel motoren opnieuw in
                                   tijd=20;
                                   draaiLinks();        //Draai tegengestelde richting
                                   gaVooruit();         //Ga stukje vooruit
                                   draaiRechts();       //Draai terug
                                   OnFwd(OUT_A);        //Ga weer stukje vooruit
                            }
                            else//Obstakel tijdens wander
                            {
                                   stop wander;         //Onderbreek wander
                                   powerL=4;            //Stel motoren opnieuw in
                                   tijd=20;
                                   draaiLinks();        //Draai tegengestelde richting
                                   OnRev(OUT_C);        //Ga stukje vooruit
                                   Wait(50);            //Doe dat halve seconde
                                   start wander;        //Herstart wander
                            }
                     }
                     else //Als rechter-bumper ingedrukt
                     {
                            PlaySound(2);
                            if (SENSOR_1<grenswaarde)//Obstakel tijdens followPath
                            {
                                   stop followPath;     //Onderbreek followPath
                                   powerR=4;            //Stel motoren opnieuw in
                                   tijd=20;
                                   draaiRechts();       //Draai tegengestelde richting
                                   gaVooruit();         //Ga stukje vooruit
                                   draaiLinks();        //Draai terug
                                   OnRev(OUT_C);        //Ga weer stukje vooruit
                            }
                            else //Obstakel tijdens wander
                            {
                                   stop wander;
                                   powerR=4;
                                   tijd=20;
                                   draaiRechts();
                                   OnFwd(OUT_A);
                                   Wait(50);
                                   start wander;
                            }
                     }
                     start followreleaser;
                }
         }
}

task followreleaser()
{
       while (true)
       {
              if (SENSOR_1<grenswaarde)//Als op zwart
              {
                     Wait(5);      //Rij nog even door (om de gleuven te negeren)
                     if (SENSOR_1<grenswaarde) // Nog steeds op zwart
                     {
                            stop avoid;          //Stop de rest
                            stop wander;
                            start avoid;         //Blijf avoiden
                            start followPath;    //Ga pad volgen
                     }
              }
       }
}

task eggreleaser()
{
       while (true)
       {
              //Als op zwart en beide bumpers zijn ingedrukt
              if (SENSOR_1<grenswaarde&&SENSOR_2&&SENSOR_3)
              {
                     //Zet alles uit om puur achteruit te gaan
                     stop avoid;
                     stop followPath;
                     stop followreleaser;
                     //Zet power op vol en rij 10 seconden achteruit
                     tijd=25;
                     powerBW=7;
                     gaAchteruit();
                     //Draai 180 graden
                     tijd=(180*24)/25;
                     draaiLinks();
                     OnFwd(OUT_C+OUT_A);
                     Wait((180*24)/10);
                     //Zet alles weer aan om eieren te gaan zoeken
                     start avoid;
                     start wander;
                     start followreleaser;
              }
       }
}

task main()
{
       initsensors();
       //start lichtgeheugen;
       start followreleaser;
       start eggreleaser;
       start avoid;
       start wander;
}