#define Turn OUT_C
#define RaiseRear OUT_A
#define RaiseFront OUT_B

#define TouchRear SENSOR_2
#define TouchFront SENSOR_1
#define Angle SENSOR_3

#define HalfTurn 192

#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 ()
{
  SetSensor (Angle, SENSOR_ROTATION);
  SetSensor (TouchRear, SENSOR_TOUCH);
  SetSensor (TouchFront, SENSOR_TOUCH);
  
  while (true)
  {
    ClearMessage();
    until (Message() != 0);
    if (Message() == BarsUpMsg) BarsUp();
    if (Message() == BarsDownTouchMsg) BarsDownTouch();
    if (Message() == RaiseMiddleMsg) RaiseMiddle();
    if (Message() == Turn180Msg) Turn180();
    if (Message() == RaiseRearBarMsg) RaiseRearBar();
    if (Message() == TestTouchFrontMsg) TestTouchFront();
  }
}

sub Turn180 ()
{
  ClearSensor (Angle);
  OnFwd (Turn);
  until (Angle >= HalfTurn);
  Off (Turn);
  SendMessage (AckMsg);
}

sub BarsUp ()
{
  //Raise Bars till stop message
  int flag=true;
  OnRev(RaiseRear+RaiseFront);
  while (flag)
  {
    ClearMessage();
    until (Message() != 0);
    if (Message() == StopFrontMsg) {Off(RaiseFront);}
    if (Message() == StopRearMsg) {Off(RaiseRear);}
    if (Message() == StopAllMsg)
    {
      Off(RaiseRear+RaiseFront);
      SendMessage (AckMsg);
      flag=false;
      break;
    }
  }
}

sub RaiseRearBar ()
{
  OnRev(RaiseRear);
  ClearMessage();
  until (Message() != 0);
  if (Message() == StopAllMsg)
  {
    Off(RaiseRear);
    SendMessage (AckMsg);
  }
}

sub TestTouchFront ()
{
  until(TouchFront==1);
  Wait(20);
  SendMessage (AckMsg);
}

sub BarsDownTouch ()
{
  int flag=0;
  OnFwd(RaiseRear+RaiseFront);
    while (flag !=3)
    {
      if ( (TouchFront==0) && ((flag & 1)==0))
      {
        flag |=1;
        Off (RaiseFront);
      }
      if ( (TouchRear==0) && ((flag & 2)==0))
      {
        flag |=2;
        Off (RaiseRear);
      }
    }
  SendMessage (AckMsg);
}

sub RaiseMiddle ()
{
  ClearTimer (0);
  ClearMessage();
  OnFwd(RaiseRear);
  Wait(50);
  OnFwd(RaiseFront);
  until (Message() != 0)
  {
    if((Timer(0)&7)==0) Off(RaiseFront);
    else OnFwd(RaiseFront);
  }

  if (Message() == StopAllMsg)
  {
    Off(RaiseFront);
    Wait(50);
    Off(RaiseRear);
    SendMessage (AckMsg);
  }
}