#define Front OUT_C
#define Rear OUT_A
#define Middle OUT_B

#define LightFront SENSOR_2
#define LightRear SENSOR_1
#define TouchMiddle SENSOR_3

#define RearThr 52
#define FrontThr 48

#define AckMsg 1
#define StopFrontMsg 2
#define StopRearMsg  3
#define StopAllMsg  4
#define BarsUpMsg 5
#define BarsDownTouchMsg 6
#define RaiseMiddleMsg  7
#define Turn180Msg  8
#define RaiseRearBarMsg 9
#define TestTouchFrontMsg 10



task main ()
{
  int i;
  SetSensor (LightFront, SENSOR_LIGHT);
  SetSensor (LightRear, SENSOR_LIGHT);
  SetSensor (TouchMiddle, SENSOR_TOUCH);
  Wait(100);
  
  for(i=0; i<3; i++)
  {
    BarsUp();
    SetPower (Middle,4);
    OnFwd (Middle);
    Wait(300);
    Off(Middle);
    SendCmd(BarsDownTouchMsg);
    RaiseMiddle ();
    SetPower (Rear+Middle+Front,3);
    OnFwd (Rear+Front);
    Wait(300);
    OnRev(Rear);
    Off(Front);
    Wait(20);
    Off(Rear+Front);
    LowerMiddle();
  }
  BarsUp();
  SetPower (Middle,3);
  OnFwd (Middle);
  Wait(300);
  Off(Middle);
  SendCmd(BarsDownTouchMsg);
  RaiseMiddle ();
  SetPower (Rear+Front,3);
  OnFwd (Rear+Front);
  Wait(300);
  Off(Rear+Front);
  SendCmd(Turn180Msg);
  LowerMiddle();
  RaiseRearBar();
  SendCmd(Turn180Msg);
    
  for(i=0; i<3; i++)
  {
    SendCmd(BarsDownTouchMsg);
    OnFwd (Rear+Middle+Front);
    until(TouchMiddle == 1);
    Wait(20);
    Off (Rear+Middle+Front);
    LowerMiddleTouch();
    OnFwd (Rear+Middle+Front);
    SendCmd(TestTouchFrontMsg);
    Wait(30);
    Off (Rear+Middle+Front);
    SendCmd(BarsDownTouchMsg);
  }
  SendCmd(BarsDownTouchMsg);
  OnFwd (Rear+Middle+Front);
  until(TouchMiddle == 1);
  Wait(20);
  Off (Rear+Middle+Front);
  LowerMiddleTouch();
  OnFwd (Rear+Middle+Front);
  SendCmd(TestTouchFrontMsg);
  Wait(30);
  Off (Rear+Middle+Front);
  PlaySound (SOUND_FAST_UP);
  PlaySound (SOUND_FAST_UP);
}


void WaitAck (int waiting)
{
  int flag = false;

  ClearMessage ();
  ClearTimer (3);
  while (Timer (3)<waiting)
  {
    if (Message () == AckMsg)
    {
      flag = true;
      break;
    }
  }
  if(flag==false)
  {
    PlaySound (SOUND_DOWN);
    StopAllTasks ();
  }
}


sub BarsUp ()
{
  SendMessage (BarsUpMsg);
  int flag = 0;
  while ((LightFront>FrontThr) || (LightRear>RearThr));
  while (flag !=3)
  {
    if ( (LightFront>FrontThr) && ((flag & 1)==0))
    {
      flag |=1;
      Wait(100);
      SendMessage (StopFrontMsg);
    }
    if ( (LightRear>RearThr) && ((flag & 2)==0))
    {
      flag |=2;
      SendMessage (StopRearMsg);
    }
  }
  SendMessage (StopAllMsg);
  WaitAck(10);
}

sub RaiseRearBar ()
{
  SendMessage (RaiseRearBarMsg);
  until (LightRear>RearThr);
  Wait(100);
  SendMessage (StopAllMsg);
  WaitAck(10);
}

void SendCmd(int Msg)
{
  SendMessage (Msg);
  WaitAck(1000);
}

sub RaiseMiddle ()
{
  SendMessage (RaiseMiddleMsg);
  until (LightRear<=RearThr);
  Wait(100);
  until (LightRear>RearThr);
  SendMessage (StopAllMsg);
  WaitAck(10);
}

sub LowerMiddle()
{
  SendMessage (BarsUpMsg);
  int flag=0;
  
  while (flag !=3)
  {
    if ( (LightFront>FrontThr) && ((flag & 1)==0))
    {
      flag |=1;
      Wait(100);
      SendMessage (StopFrontMsg);
    }
    if ( (TouchMiddle==0) && ((flag & 2)==0))
    {
      flag |=2;
      SendMessage (StopRearMsg);
    }
  }
  SendMessage (StopAllMsg);
  WaitAck(10);
}

sub LowerMiddleTouch()
{
  SendMessage (BarsUpMsg);
  until (TouchMiddle==0);
  SendMessage (StopAllMsg);
  WaitAck(10);
}