#define position_pre-firstwall_line() \
        OnRev(OUT_A+OUT_C);Wait(30);Off(OUT_A+OUT_C);OnFwd(OUT_A);Wait(110);Off(OUT_A+OUT_C);\


#define position_pre-gate_line() \
        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 opengate() \
        Wait(40); Off(OUT_A+OUT_C); OnRev(OUT_B);Wait(30);Off(OUT_B);OnFwd(OUT_B);Wait(10);\
        Off(OUT_B);Wait(50);Off(OUT_A+OUT_C);
        
#define position_pre-pickball_line() \
        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()\
        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(30);Off(OUT_B);\
        Wait(200);OnRev(OUT_B);Wait(50);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 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();
 read_sufficient_enviroment();

 SetPower(OUT_A+OUT_C,3);
 find_blackline();
 position_pre-gate_line();



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

 opengate();


 gate=true;
 Follow_Line();
 gate=false;


 find_blackline();
 position_pre-firstwall_line();


 SetPower(OUT_A+OUT_C, 2);
 Lthr=Lgrey;Rthr=Rgrey;
 Follow_Line();
 
 position_pre-pickball_line();



 turntime_fline=3;SetPower(OUT_A,1);SetPower(OUT_C,4);
 Lthr=Lblack;Rthr=Rblack;
 Follow_Line();
 
 pickball();
 
 
 turntime_fline=10;LEthr=1;REthr=1;
 Follow_Line();
 
 dropball();

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


 
 labyrint();
}

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

sub find_blackline()
{
 OnFwd(OUT_A+OUT_C);
 while(RSENSOR>Rblack+2);
}

sub read_sufficient_enviroment()
{
 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 timer_threshold=20;

 int turntime=5;
 
 while(labturns!=0)
 {
  OnFwd(OUT_A+OUT_C);
  
  ClearTimer(0);
  until(TSENSOR);
  if(Timer(0)>=timer_threshold)
  { 
    if (direction==4) { direction=0; } 
    else { direction++; }
    labturns--;
    turntime=5;
  }

  turntime+=1;
  switch(direction)
  {
   case 1:
   case 2:
    LeftLab();
   
   case 2:break;
   
   case 2:
   case 3:
    RightLab();
   break;
  }
  
  direction++;

 }
 
 
}