import josx.platform.rcx.*;

/**
 * Entry point for the program. Creates an instance of Subsumption
 * and kicks of the lowest priority task (wander)
 */
public class Main  {
	public static Subsumption main;
	public static void main (String[] arg)
	  throws Exception {
	  	main = new Subsumption();
		main.tasks[0].execute();
	}
}

/**
 * Coordinates a prioritised set of tasks. Tasks with higher priority
 * completely block those of a lower priority. Creates the following
 * tasks with increasing order of priority:
 * - Wander:      wander around
 * - RightBumber: avoid obstacles on the right.
 * - LeftBumber:  avoid obstacles on the left.
 * Plus it gates access to the effectors (i.e. the motors) ensuring
 * that it's priority policy is strictly enforced.
 */
class Subsumption implements ButtonListener {
	public boolean running;
	public int owner;
	Task tasks[];
	
	public Subsumption() {
		running = true;
		Button.RUN.addButtonListener(this);
		tasks = new Task[3];
		tasks[0]=new Wander();
		tasks[1]=new RightBumber();
		tasks[2]=new LeftBumber();
		tasks[0].start();
		tasks[1].start();
		tasks[2].start();
	}

	int getPriority(Task t) {
		for (int i=0; i<tasks.length; i++) {
			if (tasks[i] == t)
				return i;
		}
		
		return -1;
	}

	/**
	 * Arbitrates between the various tasks.
	 */	
	public synchronized void execute(Task requestor) {
		int pri = getPriority(requestor);

		// If its a lower priorty than the current task, ignore it.
		if (pri < owner)
			return;

		// This is the new owner of the single output that we have
		owner = pri;

		// Start new owner from beginning (even if it was the same one)
		tasks[owner].reset();
	}

	/**
	 * Only allow the owner to do stuff, just in case some other task calls us
	 */	
	public synchronized void setMotor(Task requestor, Motor motor, int power, boolean forward) {
		if (owner == getPriority(requestor)) {
			motor.setPower(power);
			if (forward)
				motor.forward();
			else
				motor.backward();
		}
	}

	/**
	 * Task has finished. Re-start next runnable task.
	 */	
	public synchronized void release(Task releaser) {
		int pri = getPriority(releaser);
		
		// If it isn't the owner releasing, ignore it.
		if (owner == pri) {
			// Search for the first runnable task. There is always one.
			for(int i=pri-1; i >= 0; i--) {
				if (tasks[i].running()) {
					owner = i;
					tasks[owner].reset();
				}	
			}
		}
	}

	/**
	 * Called within the scope of a thread defined by Button.addButtonListener().
	 */			
	public void buttonPressed(Button b) {
		running=false;
	}
		
	public void buttonReleased(Button b) {
	}
}

/**
 * Functor interface. Or, to put it another way, the interface to
 * actions stored in a finite state machine (fsm).
 */
interface Action {
	public int act();
}

/**
 * All tasks (wander, leftbumber, rightbumber) extend this class. This class
 * defines the lifecycle of a task. Namely:
 * Reset:   Re-initialise task
 * Execute: Attempt to run (may not succeed if a higher priority task is running).
 * Run:     Execute an FSM.
 * Release: Stop running.
 */
abstract class Task extends Thread implements SensorConstants {
	public static final Motor LEFT_MOTOR = Motor.C;
	public static final Motor RIGHT_MOTOR = Motor.A;
	public static final Sensor LEFT_BUMBER = Sensor.S3;
	public static final Sensor RIGHT_BUMBER = Sensor.S1;
	public static final boolean FORWARD = true;
	public static final boolean BACKWARD = false;
	public static final int END = -1;
	public static final int START = 0;
	
	public Action actions[];
	public int fsm[];
	public int state = END;

	/**
	 * Reset the FSM to its initial state.
	 */	
	public void reset() {
		state = START;
	}

	/**
	 * The thread entry point. Either runs the action's FSM to completion -
	 * sleeping or yielding between each state - or until 'running' is false.
	 * When finished call 'release'.
	 * <P>
	 * FSM is really a bit of a misnomer as there are no input events so
	 * there is only one transition from each state to the next.
	 */
	public void run() {
		// Keep running until the program should exit.
		do {
			// Quiesce
			while (state != START && Main.main.running) {
				yield();
			}
			
			// Execute the FSM until it stops...
			do {
				int toSleepFor;
				synchronized (Main.main) {
					toSleepFor = actions[state].act();
					state = fsm[state];
				}
				if (toSleepFor > 0) {
					try {
						sleep(toSleepFor);
					} catch (InterruptedException ie) {
					}
				}
				else	
					yield();
			} while (state != END && Main.main.running);
			
			// Its over, release the actuators.
			release();
		} while (Main.main.running);
	}

	/**
	 * Inform the coordinator that we have released the actuators.
	 */	
	public void release()  {
		Main.main.release(this);
	}

	/**
	 * Request control of the actuators
	 */	
	public void execute() {
		if (Main.main != null)
			Main.main.execute(this);
	}

	/**
	 * Return true if the FSM is executing, false otherwise.
	 */
	public boolean running()  {
		return state != END;
	}
	
	/**
	 * Convenience function to make it appear to subclasses that
	 * they have direct control of the actuators when they are in
	 * fact gated by the controller.
	 */	
	public void setMotor(Motor motor, int power, boolean forward) {
		Main.main.setMotor(this, motor, power, forward);
	}
}

/**
 * Defines a finite state machine to avoid an obstacle on the left.
 */		
class LeftBumber extends Task implements SensorListener {
	public LeftBumber() {
		LEFT_BUMBER.setTypeAndMode (SENSOR_TYPE_TOUCH, SENSOR_MODE_BOOL);
		actions = new Action[3];
		actions[0] = new Action() {
			public int act() {
				setMotor(LEFT_MOTOR, 7, BACKWARD);
				setMotor(RIGHT_MOTOR, 7, BACKWARD);
				return 200;
			}
		};
		
		actions[1] = new Action() {
			public int act() {
				setMotor(LEFT_MOTOR, 7, FORWARD);
				return 200;
			}
		};

		// Shouldn't really need to do this one, but reset()
		// may not be immediate on lower priority tasks.
		actions[2] = new Action() {
			public int act() {
				setMotor(RIGHT_MOTOR, 7, FORWARD);
				return 0;
			}
		};

		fsm = new int[3];		
		fsm[0] = 1;
		fsm[1] = 2;
		fsm[2] = END;
		LEFT_BUMBER.activate();
		LEFT_BUMBER.addSensorListener(this);
	}

	/**
	 * This is actually executed in a thread established by
	 * LEFT_BUMBER.addSensorListener().
	 */	
	public void stateChanged(Sensor bumber, int oldValue, int newValue) {
		Sound.playTone(440, 10);
		if (bumber.readBooleanValue()) {
			Sound.playTone(500, 10);
			execute();
		}
	}
}

/**
 * Defines a finite state machine to avoid an obstacle on the right.
 */		
class RightBumber extends Task implements SensorListener {
	public RightBumber() {
		RIGHT_BUMBER.setTypeAndMode (SENSOR_TYPE_TOUCH, SENSOR_MODE_BOOL);
		actions = new Action[3];
		actions[0] = new Action() {
			public int act() {
				setMotor(LEFT_MOTOR, 7, BACKWARD);
				setMotor(RIGHT_MOTOR, 7, BACKWARD);
				return 200;
			}
		};
		
		actions[1] = new Action() {
			public int act() {
				setMotor(RIGHT_MOTOR, 7, FORWARD);
				return 200;
			}
		};

		// Shouldn't really need to do this one, but reset()
		// may not be immediate on lower priority tasks.
		actions[2] = new Action() {
			public int act() {
				setMotor(LEFT_MOTOR, 7, FORWARD);
				return 0;
			}
		};

		fsm = new int[3];		
		fsm[0] = 1;
		fsm[1] = 2;
		fsm[2] = END;
		RIGHT_BUMBER.activate();
		RIGHT_BUMBER.addSensorListener(this);
	}

	/**
	 * This is actually executed in a thread established by
	 * RIGHT_BUMBER.addSensorListener().
	 */	
	public void stateChanged(Sensor bumber, int oldValue, int newValue) {
		Sound.playTone(1000, 10);
		if (bumber.readBooleanValue()) {
			Sound.playTone(1400, 10);
			execute();
		}
	}
}

/**
 * Defines a finite state machine to wander around aimlessley. Note that
 * this does not really work very well as higher priority behaviours
 * could occur in the middle of a sleep which means that once the higher
 * priority behaviour terminates the robot will continue on its last
 * trajectory.
 */		
class Wander extends Task {
	public Wander() {
		actions = new Action[3];
		actions[0] = new Action() {
			public int act() {
				setMotor(LEFT_MOTOR, 7, FORWARD);
				setMotor(RIGHT_MOTOR, 7, FORWARD);
				return 5000;
			}
		};
		
		actions[1] = new Action() {
			public int act() {
				setMotor(LEFT_MOTOR, 3, FORWARD);
				setMotor(RIGHT_MOTOR, 7, FORWARD);
				return 2000;
			}
		};
		
		actions[2] = new Action() {
			public int act() {
				setMotor(LEFT_MOTOR, 7, BACKWARD);
				setMotor(RIGHT_MOTOR, 7, FORWARD);
				return 700;
			}
		};
		
		fsm = new int[3];		
		fsm[0] = 1;
		fsm[1] = 2;
		fsm[2] = 0;
	}
}