import josx.robotics.*;
import utmech.mechanompu.actuator.*;
import utmech.mechanompu.sensor.*;

/**
 * Behavior Control example
 *  * @author <bbagnall@escape.ca>Brian Bagnall
 */
class BumperCar {
   public static void main(String [] args) {
      wheels.set_servo_mode(true);
      Behavior b1 = new DriveForward();
      Behavior b2 = new HitWall();
      // BatteryLow doesn't work well so I excluded it
      //Behavior b3 = new BatteryLow(6.0f);
      
      // NOTE: low level behaviors should have lower index number
      // in the array i.e. b2 is higher level than b1.
      Behavior [] bArray = {b1, b2};
      //Behavior [] bArray = {b1, b2, b3};
      
      Arbitrator arby = new Arbitrator(bArray);
      arby.start();
   }   
}

class DriveForward implements Behavior {

   public boolean takeControl() {
      return true;
   }

   public void suppress() {
       wheels.set_goal_lvel(0);
       wheels.set_goal_rvel(0);
   }
   
   public void action() {
      wheels.set_goal_lvel(30);
      wheels.set_goal_rvel(30);
   }
}

class HitWall implements Behavior {

   public boolean takeControl() {
      return bumper.l() || bumper.r();
   }
   
   public void suppress() {
      wheels.set_goal_lvel(0);
      wheels.set_goal_rvel(0);
   }
   
   public void action() {
      // Back up:
      wheels.set_goal_lvel(-30);
      wheels.set_goal_rvel(-30);
      try{Thread.sleep(1000);}catch(Exception e) {}
      // Rotate by causing only one wheel to stop:
      wheels.set_goal_lvel(0);
      try{Thread.sleep(300);}catch(Exception e) {}
      wheels.set_goal_rvel(0);
   }
}