#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 lab_hh() OnRev(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);OnFwd(OUT_A);Wait(110);Off(OUT_A+OUT_C);\


#define hittaturn() 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(40);Off(OUT_B);

#define LineLeft() OnFwd(OUT_A); OnRev(OUT_C); Wait(turntime_fline); Off(OUT_A+OUT_C);
#define LineRight() OnFwd(OUT_C); OnRev(OUT_A); Wait(turntime_fline); Off(OUT_A+OUT_C);

#define CarefulLineLeft() OnFwd(OUT_A);OnRev(OUT_C);Wait(5);\
                          OnRev(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);

#define CarefulLineRight() OnFwd(OUT_C);OnRev(OUT_A);Wait(5);\
                          OnRev(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);

#define MGates() Wait(40); Off(OUT_A+OUT_C); OnRev(OUT_B);Wait(30);Off(OUT_B);OnFwd(OUT_B);Wait(10);\
        Off(OUT_B);Off(OUT_A+OUT_C);
        
#define MWall() OnRev(OUT_A+OUT_C);Wait(75);Off(OUT_C);Wait(70);OnFwd(OUT_A+OUT_C);Wait(20);Off(OUT_A+OUT_C);\
                SetPower(OUT_A+OUT_C, 2);

#define PickBall() OnFwd(OUT_A+OUT_C);PlaySound(SOUND_UP);Wait(40);\
        Off(OUT_A+OUT_C);OnRev(OUT_B);Wait(20);Off(OUT_B); \
        PlaySound(SOUND_DOWN);\
        OnRev(OUT_A+OUT_C);Wait(20);OnRev(OUT_A);OnFwd(OUT_C);Wait(70);OnFwd(OUT_A+OUT_C);Wait(40);
        
#define PickBall2() Off(OUT_A+OUT_C);SetPower(OUT_A+OUT_C,2);OnFwd(OUT_A+OUT_C);Wait(40);\
                    Off(OUT_A+OUT_C);OnRev(OUT_B);Wait(20);Off(OUT_B);Off(OUT_A+OUT_C);OnRev(OUT_A+OUT_C);\
                    Wait(80);OnFwd(OUT_C);OnRev(OUT_A);Wait(40);Off(OUT_A+OUT_C);OnFwd(OUT_A+OUT_C);Wait(80);

        
#define DropBall() Off(OUT_A+OUT_C);OnFwd(OUT_B);Wait(20);OnFwd(OUT_B);Wait(30);Off(OUT_B);\
        Wait(200);OnRev(OUT_B);Wait(40);Off(OUT_B);OnRev(OUT_A+OUT_C);Wait(60);Off(OUT_A+OUT_C); \
        OnRev(OUT_A+OUT_C);Wait(60);OnFwd(OUT_A);Wait(100);Off(OUT_A+OUT_C);

#define OppnaGrind() OnRev(OUT_B); Wait(time_liftdown); Off(OUT_B);\
                     OnRev(OUT_A+OUT_C); Wait(50); Off(OUT_A+OUT_C);\
                     OnFwd(OUT_C);Wait(time_liftdown);Off(OUT_B);



#define StopLab() Off(OUT_A+OUT_C);

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

#define InitSensors() SetSensor(SENSOR_1, SENSOR_LIGHT);SetSensor(SENSOR_3, SENSOR_LIGHT);SetSensor(SENSOR_2, SENSOR_TOUCH);

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



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

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

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

//Miscellaneous

int gate,turntime_fline=10,LabTimer=0;

task main()
{

 InitSensors();
 GetTimerValues();

 SetPower(OUT_A+OUT_C,3);
 FindBlack();

 hittaturn();


 Lthr=Lblack;Rthr=Rblack;LEthr=Lgrey;REthr=Rgrey;
 linje_till();
 MGates();


 gate=true;
 linje_till();
 gate=false;


 FindBlack();
 lab_hh();


 SetPower(OUT_A+OUT_C, 2);
 Lthr=Lgrey;Rthr=Rgrey;
 linje_till();
 MWall();



 turntime_fline=3;SetPower(OUT_A,1);SetPower(OUT_C,4);
 Lthr=Lblack;Rthr=Rblack;

 linje_till();
 PickBall2();
 
 
 turntime_fline=10;LEthr=1;REthr=1;
 linje_till();
 DropBall();

 SetPower(OUT_A+OUT_C,3);
 
 Lthr=Lgrey;Rthr=Rgrey;
 linje_till();

 PlaySound(SOUND_CLICK);PlaySound(SOUND_CLICK);PlaySound(SOUND_CLICK);PlaySound(SOUND_CLICK);
 
 FindWhite();

 
 labyrint();
}

sub linje_till()
{
while
 (
  !(
    ( (LSENSOR>LEthr-2 && LSENSOR<LEthr+2) && (RSENSOR>REthr-2 && RSENSOR<REthr+2 ) ) /*Slutlinje träffad*/
    |
    (TSENSOR && gate==false) /*Trycksensor träffad*/
    )
 )
 {
  OnFwd(OUT_A+OUT_C);
  if( (RSENSOR<Rthr+2) ) { LineRight(); }
  else if ( (LSENSOR<Lthr+2) ) { LineLeft(); }
 }
}

void FindBlack()
{
 OnFwd(OUT_A+OUT_C);
 while(RSENSOR>Rblack+2);
}

void FindWhite()
{
 PlaySound(SOUND_DOWN);
 OnRev(OUT_A+OUT_C);
 while(LSENSOR<Lwhite-1) { LabTimer++; Wait(1); }
 PlaySound(SOUND_UP);
 Off(OUT_A+OUT_C);
}


sub GetTimerValues()
{
 until(TSENSOR);Rwhite=RSENSOR;Lwhite=LSENSOR;Wait(20);PlaySound(SOUND_CLICK);
 until(TSENSOR);Rgrey=RSENSOR;Lgrey=LSENSOR;Wait(20);PlaySound(SOUND_CLICK);
 until(TSENSOR);Rblack=RSENSOR;Lblack=LSENSOR;Wait(20);PlaySound(SOUND_CLICK);
  
 until(TSENSOR);Wait(20);

}

sub labyrint()
{
 int labturns=9;
 int direction=1;
 int turntime=40;
 
 while(labturns!=0)
 {
  OnFwd(OUT_A+OUT_C);Wait(LabTimer);

  direction++;
  switch(direction)
  {
   case 1:
   case 2:
    OnRev(OUT_A);
    Wait(turntime);
   break;

   case 3:
    OnRev(OUT_C);
    Wait(turntime);
   break;

   case 4:
    OnRev(OUT_A);
    Wait(turntime);
    direction=0;
   break;
  }
  
  direction++;

 }
 
 
}