#define CenterLegSensor IN_1
#define RightLegSensor  IN_2 
#define LeftLegSensor   IN_3 

#define RightLegMotor  OUT_A 
#define LeftLegMotor   OUT_B 
#define CenterLegMotor OUT_C 

#define MINSPEED 25
#define MOVELEGS 120

int counter, tachoReading, rightLegPos, leftLegPos;

// Move left legs
sub reverseLeft {
  if(leftLegPos==0) {
    Fwd(LeftLegMotor,OUT_FULL);
    wait(LeftLegSensor==1);
    ClearTachoCounter(LeftLegMotor);
    do tachoReading = TachoCount(LeftLegMotor);
    while(tachoReading<MOVELEGS);
    Off(LeftLegMotor);
    leftLegPos=1;
  } else {
    Rev(LeftLegMotor,OUT_FULL);
    wait(LeftLegSensor==1);
    ClearTachoCounter(LeftLegMotor);
    do tachoReading = TachoCount(LeftLegMotor);
    while(tachoReading<MOVELEGS);
    Off(LeftLegMotor);
    leftLegPos=0;
  }
}

// Move right legs
sub reverseRight {
  if(rightLegPos==0) {
    Fwd(RightLegMotor,OUT_FULL);
    wait(RightLegSensor==1);
    ClearTachoCounter(RightLegMotor);
    do {tachoReading = TachoCount(RightLegMotor);}
    while(tachoReading<MOVELEGS);
    Off(RightLegMotor);
    rightLegPos=1;
  } else {
    Rev(RightLegMotor,OUT_FULL);
    wait(RightLegSensor==1);
    ClearTachoCounter(RightLegMotor);
    do {tachoReading = TachoCount(RightLegMotor);}
    while(tachoReading<MOVELEGS);
    Off(RightLegMotor);
    rightLegPos=0;
  }
}

// Lift left side of body
sub liftLeft {
  Rev(CenterLegMotor,OUT_FULL);
  wait(CenterLegSensor==1);
  wait(CenterLegSensor==0);
  PlaySound(1);
  Off(CenterLegMotor);
}

// Lift right side of body
sub liftRight {
  Fwd(CenterLegMotor,OUT_FULL);
  wait(CenterLegSensor==1);
  wait(CenterLegSensor==0);
  PlaySound(1);
  Off(CenterLegMotor);
}

// Oscillate external motor back and forth to find the center position
// Relies on some external task to stop it
task oscillateExternalMotor {
  counter=0;

  while(true) {
    counter+=20;
    Rev(CenterLegMotor,1);
    Sleep(counter);
    Fwd(CenterLegMotor,1);
    Sleep(counter);
  }
}

task main {
  Sensor(CenterLegSensor,IN_SWITCH);
  Sensor(RightLegSensor,IN_SWITCH);
  Sensor(LeftLegSensor,IN_SWITCH);

  // Init the center legs so we know where we have them
  start oscillateExternalMotor;
  wait(CenterLegSensor==1);
  stop oscillateExternalMotor;
  Off(CenterLegMotor);

  liftLeft();
  
  // Init left legs
  Rev(LeftLegMotor,1);
  Sleep(15);
  // Run the motor until the legs hits something
  do tachoReading = TachoSpeed(LeftLegMotor);
  while(tachoReading>MINSPEED);
  Off(LeftLegMotor);
  leftLegPos=0;

  liftRight();
  
  // Init right legs
  Rev(RightLegMotor,1);
  Sleep(15);
  // Run the motor until the legs hits something
  do tachoReading = TachoSpeed(RightLegMotor);
  while(tachoReading>MINSPEED);
  Off(RightLegMotor);
  rightLegPos=0;

  // Walk!
  while(true) {
    reverseRight();
    reverseLeft();
    liftLeft();
    reverseLeft();
    reverseRight();
    liftRight();
  }
}