#define Fwdleft() \
 T_path=3;T_wait=(timervl_selflngth/3);turn(); \
 T_path=1;T_wait=(timervl_360dg/4); turn();
#define Fwdright() \
 T_path=3;T_wait=(timervl_selflngth/3);turn(); \
 T_path=2;T_wait=(timervl_360dg/4); turn();

#define MotorsOff() Off(OUT_A+OUT_C);
#define MotorsOn() OnFwd(OUT_A+OUT_C);

#define LSENSOR SENSOR_1
#define TSENSOR SENSOR_2
#define RSENSOR SENSOR_3

#define WaitTsens() while(!(SENSOR==1));

/* TABLE OF GLOBAL VARIABLES, SUBSTITUE FOR FUNCTION ARGUMENT INEPTABILITY */

//Sensor values
int Lwhite, Rwhite, Lwgrey, Rwgrey, Lgrey, Rgrey, Lblack, Rblack;

// turn arguments
int T_wait, T_path;

// linje_till_grind arguments
int Lthr, Rthr, LEthr, REthr, mode;

// gettimervalues retvals
int timervl_360dg, timervl_selflngth;


task main()
{

 SetSensor(SENSOR_1, SENSOR_LIGHT);
 SetSensor(SENSOR_3, SENSOR_LIGHT);
 SetSensor(SENSOR_2, SENSOR_TOUCH);

 //GetTimerValues();

 //49, 51
 Rwgrey=49;
 Lwgrey=51;
 lab2();
 lab3();
/*


 hitta();           //hittar forsta linjen  och sänker gaffeln till grinden

 Lthr=43;Rthr=43;LEthr=45;REthr=48;mode=1;
 linje_till_grind();
 oppnagrind();                     //öpnar grind

 linje_till_grind();
 linje_till_grind();

 OnFwd(OUT_A);
 Wait(4);
 Off(OUT_A);
 fortsatt();
 lab_hh();

 Lthr=50;Rthr=47;LEthr=45;REthr=48;mode=2;
 linje_till_grind();

 SetPower(OUT_A+OUT_C, 2);



 Lthr=43;Rthr=43;LEthr=45;REthr=48;mode=3;
 linje_till_grind();
 pickball();
 lab_vv();


 Lthr=43;Rthr=43;LEthr=45;REthr=48;mode=4;
 linje_till_grind();


 bak();

 SetPower(OUT_A+OUT_C,2);
 Lthr=50;Rthr=0;mode=5;
 linje_till_grind();
 labyrint();
 */



         // lab står för laburint, och v för vänster, b för bak, och h för höger
 }

sub turn()
{
 Off(OUT_A+OUT_C);
 switch(T_path)
 {
  case 1:
    OnFwd(OUT_C);
    OnRev(OUT_A);
   break;
  case 2:
    OnFwd(OUT_A);
    OnRev(OUT_C);
   break;
  case 3:
    OnFwd(OUT_A+OUT_C);
   break;
  case 4:
    OnRev(OUT_A+OUT_C);
   break;
 }
 /*
 Stäng av motor efter N/100 sekunder
 */
 if (T_wait!=0) { Wait(T_wait); Off(OUT_A+OUT_C); }

}












void bak()
{

          OnRev(OUT_A+OUT_C);
          Wait(60);
          OnFwd(OUT_A);
          Wait(50);
          Off(OUT_A+OUT_C);
          /*
          while( (SENSOR_1>45+2) )
          { OnRev(OUT_A+OUT_C); }
          Off(OUT_A);
          Wait(40);
          Off(OUT_C);
          */


}

            void pickball()
            {
             SetPower(OUT_B,1);
             OnFwd(OUT_B);
             Wait(10);
             OnRev(OUT_B);
             Wait(30);
             Off(OUT_B);
             }

void dropball()
{
  SetPower(OUT_B,1);
  OnRev(OUT_B);
  Wait(10);
  OnFwd(OUT_B);
  Wait(30);
  Off(OUT_B);



}


 void lab_vv()
 {

              OnRev(OUT_A+OUT_C);
              Wait(20);
              OnRev(OUT_A);
              OnFwd(OUT_C);
              Wait(70);
              OnFwd(OUT_A+OUT_C);

              Wait(40);
              SetPower(OUT_A,3);
              SetPower(OUT_C,2);

 }
 void lab_hh()
 {

              OnRev(OUT_A+OUT_C);
              Wait(20);

              Off(OUT_A+OUT_C);
              OnFwd(OUT_A);
              Wait(110);
              Off(OUT_A+OUT_C);
 }

void fortsatt()
{
                     SetPower(OUT_A +OUT_C, OUT_FULL);

                     while(SENSOR_3>43)
                    {  OnFwd(OUT_A+OUT_C); }

                    Off(OUT_A+OUT_C);
                    }

void hitta ()
{
     int x;
     int z;
     int go=1;
             while(go==1)
             {
     SetPower(OUT_C,3);
     SetPower(OUT_A,3);
     SetSensor(SENSOR_1, SENSOR_LIGHT);
     SetSensor(SENSOR_3, SENSOR_LIGHT);

     z=SENSOR_1;
     x=SENSOR_3;

               OnFwd(OUT_A+OUT_C);
                 Wait(1);
                 if (x<43)
                 {
                 // då är svart linje hittad
                    OnRev(OUT_A+OUT_C);
                    Wait(40);
                    Off(OUT_A+OUT_C);
                    OnFwd(OUT_C);
                    Wait(140);
                    Off(OUT_A+OUT_C);
                    OnFwd(OUT_B);
                    Wait(30);
                    Off(OUT_B);
                    go=0;
                 }
     }

}
 void hitta2 ()
{
 /* Positionera robot inför linje: Kör framåt medans svart linje inte är hittad*/
 T_path=3;T_wait=0;turn();
 until (LSENSOR<Lblack+2);
 /* Positionera robot inför linjeföljning: Kör framåt 1/3 av robotens längd, Sväng därefter 90 grader vänster*/
 T_path=3;T_wait=(timervl_selflngth/4);turn();
 T_path=1;T_wait=(timervl_360dg/4);turn();

}

         sub linje_till_grind()
         {
         int z;
         int x;
         int go=1;
             while(go==1)
             {




              z=SENSOR_1;
               x=SENSOR_3;

                OnFwd(OUT_A+OUT_C);
if( (Rthr!=0) && (SENSOR_1<Rthr) )
{
OnFwd(OUT_A);
                                    OnRev(OUT_C);
                                    Wait(10);
                                    Off(OUT_A+OUT_C);
                                    }
                              else if ( (Lthr!=0) && (SENSOR_3<Lthr)  )
                             {
                             OnFwd(OUT_C);
                             OnRev(OUT_A);
                             Wait(10);
                             Off(OUT_A+OUT_C);
                             }

                          switch(mode)
                          {
                           case 1:
                            if ( (SENSOR_1>LEthr-2 && SENSOR_1<LEthr+2) && (SENSOR_3>REthr-2 && SENSOR_3<REthr+2 ) )
                            {
                             Wait(40);
                             Off(OUT_A+OUT_C);
                             go=0;
                            }
                            break;

                          case 2:
                           if (SENSOR_2==1)
                           {

                            OnRev(OUT_A+OUT_C);
                            Wait(40);
                            Off(OUT_C);
                            Wait(70);
                            OnFwd(OUT_A+OUT_C);
                            Wait(20);
                            Off(OUT_A+OUT_C);
                            go=0;
                           }
                           break;
                          case 3:
                           //s1h51 s3v53
                           if  ( (SENSOR_1>LEthr-2 && SENSOR_1<LEthr+2) && (SENSOR_3>50) )
                            {

                               SetPower(OUT_A+OUT_C,2);
                               OnFwd(OUT_A);
                               OnRev(OUT_C);
                               Wait(5);
                               OnRev(OUT_A+OUT_C);
                               Wait(20);
                               Off(OUT_A+OUT_C);

                            }
                           if  ( (SENSOR_3>REthr-2 && SENSOR_3<REthr+2) && (SENSOR_1>50) )
                            {
                               SetPower(OUT_A+OUT_C,2);
                               OnRev(OUT_A);
                               OnFwd(OUT_C);
                               Wait(5);
                               OnRev(OUT_A+OUT_C);
                               Wait(20);
                               Off(OUT_A+OUT_C);
                            }


                            if ( (SENSOR_1>LEthr-2 && SENSOR_1<LEthr+2)&&(SENSOR_3>REthr-2 && SENSOR_3<REthr+2 ) )
                            {
                             Wait(13);
                             Off(OUT_A+OUT_C);
                             go=0;
                            }
                            break;
                           case 4:
                             if (SENSOR_2==1)
                           {
                            Off(OUT_A+OUT_C);
                            SetPower(OUT_B,2);
                            OnRev(OUT_B);
                            Wait(5);
                            OnFwd(OUT_B);
                            Wait(30);
                            Off(OUT_B);
                            Wait(200);
                            OnRev(OUT_B);
                            Wait(60);
                            Off(OUT_B);
                            OnRev(OUT_A+OUT_C);
                            Wait(60);
                            Off(OUT_A+OUT_C);

                            go=0;
                            break;
                           }
                            case 5:
                            if (SENSOR_2==1)
                             {
                             Off(OUT_A+OUT_C);
                             go=0;
                            }

                            break;


                          }


                    }

            }



sub linje_till()
{

int go=1;

While(go==1)
 {
  OnFwd(OUT_A+OUT_C);

  if( (Rthr!=0) && (RSENSOR<Rthr) )
  {
  OnFwd(OUT_A);
  OnRev(OUT_C);
  Wait(10);
  Off(OUT_A+OUT_C);
  }

  else if ( (Lthr!=0) && (SENSOR_3<Lthr)  )
  {
   OnFwd(OUT_C);
   OnRev(OUT_A);
   Wait(10);
   Off(OUT_A+OUT_C);
  }

  switch(mode)
  {
   case 1:
    if ( (SENSOR_1>LEthr-2 && SENSOR_1<LEthr+2) && (SENSOR_3>REthr-2 && SENSOR_3<REthr+2 ) )
     {
      Wait(40);
      Off(OUT_A+OUT_C);
      go=0;
     }
     break;

   case 2:
   if (SENSOR_2==1)
   {
    OnRev(OUT_A+OUT_C);
    Wait(40);
    Off(OUT_C);
      Wait(70);
    OnFwd(OUT_A+OUT_C);
    Wait(20);
    Off(OUT_A+OUT_C);
        go=0;
   }
     break;

 case 3:
   //s1h51 s3v53
   if  ( (SENSOR_1>LEthr-2 && SENSOR_1<LEthr+2) && (SENSOR_3>50) )
   {
    SetPower(OUT_A+OUT_C,2);
    OnFwd(OUT_A);
    OnRev(OUT_C);
    Wait(5);
    OnRev(OUT_A+OUT_C);
    Wait(20);
    Off(OUT_A+OUT_C);
   }
   if  ( (SENSOR_3>REthr-2 && SENSOR_3<REthr+2) && (SENSOR_1>50) )
   {
    SetPower(OUT_A+OUT_C,2);
    OnRev(OUT_A);
    OnFwd(OUT_C);
    Wait(5);
    OnRev(OUT_A+OUT_C);
    Wait(20);
    Off(OUT_A+OUT_C);
   }

   if ( (SENSOR_1>LEthr-2 && SENSOR_1<LEthr+2)&&(SENSOR_3>REthr-2 && SENSOR_3<REthr+2 ) )
   {
    Wait(13);
    Off(OUT_A+OUT_C);
    go=0;
   }
   break;

   case 4:
   if (SENSOR_2==1)
   {
    Off(OUT_A+OUT_C);
    SetPower(OUT_B,2);
    OnRev(OUT_B);
    Wait(5);
    OnFwd(OUT_B);
    Wait(30);
    Off(OUT_B);
    Wait(200);
    OnRev(OUT_B);
    Wait(60);
    Off(OUT_B);
    OnRev(OUT_A+OUT_C);
    Wait(60);
    Off(OUT_A+OUT_C);
    go=0;
   }
   break;


   case 5:

    if (SENSOR_2==1)
    {
    Off(OUT_A+OUT_C);
   go=0;
    }

   break;

    }
  }
}








void oppnagrind()
{
 OnRev(OUT_B);
 Wait(30);
 Off(OUT_B);
 OnFwd(OUT_B);
 Wait(10);
 Off(OUT_B);

 OnRev(OUT_A+OUT_C);
 Wait(50);
 Off(OUT_A+OUT_C);

}

sub labyrint()
{
     int i=0;
     int path=1;
     for (i;i<10;i++)
          {
           while (!(SENSOR_2==1)) {}
           switch (path)
          {
              case 1:
              case 2:
                   //vänstermoment
                   LabLeft();
                break;
              case 3:
                   LabRight();
                break;
              case 4:
                   //högermoment
                   LabRight();
                   path=0;
                break;
          }
          path++;
     }
}
void LabLeft()
{
 OnRev(OUT_A+OUT_C);
 Wait(20);
 OnRev(OUT_A);
 OnFwd(OUT_C);
 Wait(110);
 OnFwd(OUT_A);
}

void LabRight()
{
 OnRev(OUT_A+OUT_C);
 Wait(20);
 OnRev(OUT_C);
 OnFwd(OUT_A);
 Wait(110);
 OnFwd(OUT_C);
}


/*
void laburint()
{
SetPower(OUT_C , 2);
SetPower(OUT_A, 2);
SetSensor(SENSOR_2,SENSOR_TOUCH);
repeat(2)
{
lab_b();
lab_v(197);
lab_b();
lab_h(183);
lab_b();
lab_h(183);
lab_b();
lab_v(197);
}
lab_b();
lab_v(183);
OnFwd(OUT_A+OUT_C);
Wait(150);
Off(OUT_A+OUT_C);
}

                                 void lab_b()
                                 {
                                  int bak=80;
                                 OnFwd(OUT_A+OUT_C);
                                 until (SENSOR_2 == 1);
                                 OnRev(OUT_A+OUT_C);
                                 Wait(bak);
                                 Off(OUT_A+OUT_C);
                                 }

void lab_v(int x)
{
int sva=x;
OnFwd(OUT_C);

Wait(sva);
Off(OUT_A+OUT_C);
}

                 void lab_h(int z)
                 {
                 int sva=z;
                 OnFwd(OUT_A);

                 Wait(sva);
                 Off(OUT_A+OUT_C);
                 }

 */



sub GetTimerValues()
{


SetLights:
 //Lwhite, Rwhite, Lgrey, Rgrey, Lblack, Rblack;
 until(TSENSOR);
  Rwhite=RSENSOR;Lwhite=LSENSOR;
  Wait(20);//PlaySound(SOUND_UP);

 until(TSENSOR);
  Rwgrey=RSENSOR;Lwgrey=LSENSOR;
  Wait(20);//PlaySound(SOUND_UP);

 until(TSENSOR);
  Rgrey=RSENSOR;Lgrey=LSENSOR;
  Wait(20);//PlaySound(SOUND_UP);

 until(TSENSOR);
  Rblack=RSENSOR;Lblack=LSENSOR;
  Wait(20);//PlaySound(SOUND_UP);



SetCircular:
 /* Undersök tid i (decisekunder*10 == Funktionen wait's resolution) för robot att svänga 90 grader */


 until(TSENSOR);                       //Vanta pa manuell forberedning
                         //Nollstall tidtagning
 SetPower(OUT_A+OUT_C,4);              //Sväng runt vanster axel
 OnRev(OUT_A);
 OnFwd(OUT_C);


/*
 until(LSENSOR>Lblack+2);                //Vanta tills sensorer traffar svart utgångslinje medans timer:0 inkrementeras
 until( (RSENSOR<Rblack+2) && (LSENSOR<Lblack+2) );
 
 until(TSENSOR);
*/

 until(LSENSOR>Lblack+2);                //Vanta tills sensorer traffar svart utgångslinje medans timer:0 inkrementeras
 until( (RSENSOR<Rblack+2) && (LSENSOR<Lblack+2) );

 Off(OUT_A+OUT_C);
 until(TSENSOR);
 ClearTimer(0);
 OnRev(OUT_A);OnFwd(OUT_C);
 
 until(LSENSOR>Lblack+2);               //Vanta tills sensorer traffar svart utgångslinje medans timer:0 inkrementeras
 until( (RSENSOR<Rblack+2) && (LSENSOR<Lblack+2) );
 
 timervl_360dg=(Timer(0) * 10);        //Spara varde ur timer:0
 Off(OUT_A+OUT_C);                     //Stanna Motorer

 /* Undersök tid i (decisekunder*10 == Funktionen wait's resolution) för robot att köra sin egen längd */

SetSelfLength:
 until(TSENSOR);                       //Vanta pa manuell forberedning
 ClearTimer(0);                        //Nollstall tidtagning

 OnFwd(OUT_A+OUT_C);


 until(LSENSOR<=Lblack+1);             //Vanta tills ljussensor blir sann medans timer:0 inkrementeras


 timervl_selflngth=(Timer(0) * 10);    //Spara varde ur timer:0

 Off(OUT_A+OUT_C);


 until(TSENSOR);                       //Vanta pa manuell forberedning

}


int timervalue;

void lab2()
{
 SetPower(OUT_A+OUT_C,4);

 until(TSENSOR);
  ClearTimer(0);
 until(TSENSOR);
  timervalue=Timer(0);


//  T_path=3;T_wait=(timervalue/2);turn();
//  T_path=1;T_wait=(timervl_360dg/4);turn();

 
}

void lab3()
{

 int MotorPower=4;
 int MagicNumber=((timervalue/2)*10);
 
 OnFwd(OUT_A+OUT_C);


 while (MotorPower!=0)
 {

  int counter=0;
  until(counter==MagicNumber) { Wait(1); counter++; }

/*
    if (MotorPower<7)
    {
     PlaySound(SOUND_CLICK);
     OnRev(OUT_C); SetPower(OUT_C,(7-MotorPower));
    }
*/
    
    MotorPower--;
    PlaySound(SOUND_CLICK); SetPower(OUT_C,MotorPower);


//  MagicNumber-=1;
  

  
 }
 

}