/* wissellaar (links en rechts motor a richting omwisselen)

(tussen haakjes is linkerkant)

pseudo code


main task
     init
     reset
         ga naar terug (heen) spoor
         sein naar beneden
     loop
         wacht op schakelaar 2
         sein omhoog
         ga naar heen (terug) spoor
         wacht tot atv weg is (delay)
         ga naar terug (heen) spoor
     voor altijd

hoho task
     als schakelaar 3
        motor a/b uit
        knipperen (c)
        piepen
     anders
        herstel motor a/b
voor altijd
*/

    int a;                                //heen-terug motor
    int b;                                //sein hoog/laag motor
    int c;                                //hoho knipper
    int omhoog;
    int omlaag;
    int sein;
    int vlag;                             //hoho vlag

    sub init()
    {
    SetSensor(SENSOR_1,SENSOR_TOUCH);
    SetSensor(SENSOR_2,SENSOR_TOUCH);
    SetSensor(SENSOR_3,SENSOR_TOUCH);
    SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
    SetSensorMode(SENSOR_2,SENSOR_MODE_RAW);
    SetSensorMode(SENSOR_3,SENSOR_MODE_RAW);
    a=0;
    b=0;
    c=0;
    return;
    }

    sub reset()
    {
    b=-1;
    OnRev(OUT_B);        // sein omlaag
    Wait(200);
    Off(OUT_B);
    b=0;
    return;
    }


    sub sein_omhoog()
    {
    b=-1;
    OnRev(OUT_B);        // sein omhoog
    Wait(200);
    Off(OUT_B);
    b=0;
    sein=1;
    return;
    }

    sub sein_omlaag()
    {
    b=1;
    OnFwd(OUT_B);        // sein omlaag
    Wait(200);
    Off(OUT_B);
    b=0;
    sein=0;
    return;
    }

  task show_sein()
  {
  while (true)
  {
//  Wait(100);
  Off(OUT_A);
  Off(OUT_C);
  Wait(100);
  if (sein==1) OnFwd(OUT_C);
  if (sein==0) OnFwd(OUT_A);
  Wait(10);
  }
  } // end task show
  
  
  
  task hoho()
  {
//  SetSensor(SENSOR_3,SENSOR_TOUCH);
  vlag=0;
  while (true)
  {

  while (SENSOR_3<200)
  {
  if (a!=0) Off(OUT_A);
  if (b!=0) Off(OUT_B);
  OnFwd(OUT_C);
  PlayTone(220,10);
  Wait(30);
  Off(OUT_C);
  Wait(30);
  PlayTone(110,10);
  vlag=1;
  }

  if (vlag==1)
  {
  if (a==1)  OnFwd(OUT_A);
  if (a==-1) OnRev(OUT_A);
  if (b==1)  OnFwd(OUT_B);
  if (b==-1) OnRev(OUT_B);
  vlag=0;
  }

  } //while forever
  } //hoho
  
  task alarm()
  {
  Off(OUT_A);
  Off(OUT_B);
  while (true)
  {
  OnFwd(OUT_C);
  PlayTone(220,10);
  Wait(10);
  Off(OUT_C);
  PlayTone(110,10);
  Wait(10);
  }
  }

//**********************************************************

  task main()
  {
  
    init();
    start hoho;
    start show_sein;
    reset();
    sein_omhoog();
    
    while (true)
    {
  
      if (SENSOR_2>300)   //start ATV gezien
      {
      omhoog=0;
      while (omhoog==0)
      {
      if (SENSOR_1>300)
      {
       if (sein==1) sein_omlaag();
      }
      if (SENSOR_2<300) omhoog=1;
      }
      if (sein==0)
      {
      Wait(200);             // wacht tot ATV voorbij is
      sein_omhoog();         // blokkeren
      }
      }

      if (SENSOR_1>300)  // groen
      {
      while (SENSOR_1>300);
      if (sein==1) sein_omlaag();    // vrijgeven
      }

   } // do forever
     
  } //end main