/*
 * This program version will actually attempt navigation through a maze.
 * 
 * 
 * A modestly clueful user is required to first press the gray button three
 * times to set the sensor values for the background of the maze (white).  Then,
 * press the black button three times to set the track lightness values.
 * Finally, the RUN button starts the main program.
 * 
 * For now I will ignore whether it can get into an infinite loop if there is a
 * loop in the maze.  I will leave that array in place and an empty method for
 * that, anyway.
 *
 * 
 * *******************
 * Julie Powers-Boyle
 * 20060226 Sun, Programming WS
 * $Id
 * *******************
 */

import josx.platform.rcx.*;
import josx.util.*;
import java.util.Random;

public class MazeNav implements SensorConstants{
	
	// different values since there are differences among sensors
	static int backgroundValues[] = {0,0,0};
	static int trackValues[] = {0,0,0};
	static int finishValues[] = {0,0,0}; // if this value is found, it finishes
	static int coordinates[] = {0,0}; // [x,y] as estimated by motor duration
	static int direction; // right, left, up, down
	static int choiceLog[][]; // can I add to the size of this?
	static int timeLog[]; // and to this?
	static int stepNumber; // this serves as an index number for arrays
	static int currentTime;
	static Timer chrono = new Timer(10, new StopTimer()); // too often? sparse?
	static Random random = new Random(3);
	
	public static void main (String args[]) throws InterruptedException{
	
		activateRCX();
		setLightValues();
	
		//Don't run away until someone says to do so
		TextLCD.print("RUN?");
		Button.RUN.waitForPressAndRelease();
		
		// Now start; this does everything
		explore();
		
		finish();
		
		Button.RUN.waitForPressAndRelease();
	
	}  // closing main

	// massively decomposed methods:
	
	// Setup: activateRCX and setLightValues
	static void activateRCX(){
		
		for (int i = 0; i <= 2; i++){
			Sensor.SENSORS[i].setTypeAndMode(SENSOR_TYPE_LIGHT,
				SENSOR_MODE_PCT);
			Sensor.SENSORS[i].activate();
		}
		for (int i = 0; i <= 2; i++){
			Sensor.SENSORS[i].addSensorListener(new LightWatcher());
		}
		Motor.A.setPower(3);  // just to start
		Motor.C.setPower(3);
	}
	
	static void setLightValues() throws InterruptedException{
	
		// I'm making this much more explicit than absolutely necessary, because
		// I want to see the values of the different-voltaged sensors.
		
		// This idea adapted from Jim's LineWalker.java
		String displayThis;
		int i;
		
		TextLCD.print("P 0");  // P for paper
		for (i = 0; i <= 2; i++){
			Button.PRGM.waitForPressAndRelease();
			backgroundValues[i] = Sensor.SENSORS[i].readValue();
			displayThis = "P" + Integer.toString(i+1) + " " 
				+ Integer.toString(backgroundValues[i]);
			TextLCD.print(displayThis);
		}
		Thread.sleep(1000);
		
		// yes, I could probably nest these loops and fish with an array
		TextLCD.print("T 0");
		for (i = 0; i <= 2; i++){
			Button.VIEW.waitForPressAndRelease();
			trackValues[i] = Sensor.SENSORS[i].readValue();
			displayThis = "T" + Integer.toString(i+1) + " " 
				+ Integer.toString(trackValues[i]);
			TextLCD.print(displayThis);
		}
		Thread.sleep(1000);
		
		TextLCD.print("F 0");
		for (i = 0; i <= 2; i++){
			Button.PRGM.waitForPressAndRelease();
			finishValues[i] = Sensor.SENSORS[i].readValue();
			displayThis = "F" + Integer.toString(i+1) + " " 
				+ Integer.toString(finishValues[i]);
			TextLCD.print(displayThis);
		}
		Thread.sleep(1000);
	}
	
	// Umbrella activity: explore
	static void explore() throws InterruptedException{
		// Thread moving forward with waiting for an event where the sensors
		// change, in which case there is a decision to be made about direction.
		
		while (true){
			moveForward();
			try {
				Thread.sleep(10000); // max time between events: 10s
			} catch (InterruptedException e) {
				completeStop(true);
			}
			if (areWeOnTrack() == false){
			} else {
				updateChoiceLogSensors();
				updateCoordinates();
				while (newWayToGo() == false){
					backOutFromDeadEnd();
				}
				chooseDirection();
				turnCorrectly();
				for (int i = 0; i < 3; i++) { // break for finish
					if (choiceLog[stepNumber-1][i] == 3) {
						return;
					}
				}
				stepNumber++;
			}
			
		}
	}
	
	// Movement: moveForward, moveBackward, turnLeft, turnRight, completeStop
	static void moveForward(){
		activateTimer();
		Motor.A.forward();
		Motor.C.forward();
	}
	
	static void moveBackward(){
		// timing all in backOutFromDeadEnd()
		Motor.A.backward();
		Motor.C.backward();
	}
	
	static void turnLeft() throws InterruptedException{
		Motor.A.backward();
		Motor.C.forward();
		Thread.sleep(1000);	 // this needs calibration
		TextLCD.print("heh");
		completeStop(false);
		updateDirection(0);
	}
	
	static void turnRight() throws InterruptedException{
		Motor.A.forward();
		Motor.C.backward();
		Thread.sleep(1000);	 // this needs to be calibrated
		TextLCD.print("SPLAT");
		completeStop(false);
		updateDirection(2);
	}
	
	static void completeStop(boolean wentForward) throws InterruptedException{
		// and stops the chrono timer if robot had been forward
		Motor.A.stop();
		Motor.C.stop();
		if (wentForward == true) {
			stopTimer();
		}
		Thread.sleep(500);
	}
	
	// Timer: activateTimer, timedOut (in StopTimer), stopTimer
	static void activateTimer(){
		currentTime = 0;  // reset stopwatch
		chrono.start();
	}
	
	static void stopTimer(){
		chrono.stop();
		updateTimeLog();
	}
	
	// Sensors: (now in LightWatcher)
	
	// Decisions: chooseDirection, backOutFromDeadEnd
	static void chooseDirection(){
		// If presented with more than one directional option, this will pick
		// one at random (sort of).

		int someNum = random.nextInt(); // an int under 3
		
		for (int i = 0; i < 3; i++){
			if(choiceLog[stepNumber][someNum] == 1){
				choiceLog[stepNumber][3] = someNum;
				return;
			} else if (someNum != 2){
				someNum++;
			} else {
				someNum = 0;
			}
		}
		TextLCD.print("baddir");
		try{
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
			System.exit(1);
		}
	}
	
	static void backOutFromDeadEnd() throws InterruptedException{
		// If the robot finds itself in a dead end, its navigation plan is a
		// little bit different from its usual one.
		
		Timer preSet = new Timer(timeLog[stepNumber], new StopTimer());
		preSet.start();
		moveBackward();
		try {
			Thread.sleep(10000);
		} catch (InterruptedException e) {
			completeStop(false);
			turnOpposite();
			stepNumber--;
			choiceLog[stepNumber][(choiceLog[stepNumber][3])] = 2; // bad route
		}
	}
	
	// Navigational discerners: areWeOnTrack, newWayToGo
	static boolean areWeOnTrack() throws InterruptedException{
		// If the last time in the array is too small, it will assume it has
		// veered off course and will wait for human intervention.	Then, it 
		// picks up from the last intersection.
		
		if (timeLog[stepNumber] < 500) {
			TextLCD.print("Off");
			Sound.playTone(110, 20);
			Button.RUN.waitForPressAndRelease(); // put it on last intersection
			return false;
		}
		return true;
	}
	
	static boolean newWayToGo(){
		// Tests if this is a dead end.
		
		for (int i = 0; i <= 2; i++){
			if (choiceLog[stepNumber][i] == 1) {
				return true;
			}
		}
		return false; // if all sensors read something unnavigable
	}
	
	// Translation: whatColorIsIt, turnCorrectly, turnOpposite
	static int whatColorIsIt(int whichSensor) throws InterruptedException {
		// This determines what the sensor is seeing, and tells the choiceLog
		
		int lightness = Sensor.SENSORS[whichSensor].readValue();
		
		if (Math.abs(lightness - backgroundValues[whichSensor]) < 2) {
			return 0;
		} else if (Math.abs(lightness - trackValues[whichSensor]) < 2) {
			return 1;
		} else if (Math.abs(lightness - finishValues[whichSensor]) < 2) {
			return 3;
		} else {
			TextLCD.print("blind");
		}
		try{
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
		}
		System.exit(1);
		return 4; // Does exit not work, then?
	}
	
	static void turnCorrectly() throws InterruptedException{
		if (choiceLog[stepNumber][3] == 0){
			turnLeft();
		} else if (choiceLog[stepNumber][3] == 2) {
			turnRight();
		}
	}
	
	static void turnOpposite() throws InterruptedException {
		if (choiceLog[stepNumber][3] == 0){
			turnRight();
		} else if (choiceLog[stepNumber][3] == 2) {
			turnLeft();
		}
	}
	
	// Write: updateChoiceLogSensors, updateTimeLog, updateDirection,
		// updateCoordinates
	static void updateChoiceLogSensors() throws InterruptedException {
		// Enlightens this array to the most recent sensor values.
		
		for (int i = 0; i <= 2; i++) {
			choiceLog[stepNumber][i] = whatColorIsIt(i);
		}
	}
	
	static void updateTimeLog(){
		// Whenever it stops, it will put for how many hundredths of seconds the
		// motor had been on then.
		
		timeLog[stepNumber] = currentTime;
	}
	
	static void updateDirection(int turn){
		// This updates the direction variable, keeping track of its actual
		// orientation.  The following integers denote the following directions:
		// 0 left; 1 up; 2 right; 3 down
		// These keep the values of left and right consistent.
		// This is not yet used for a haveWeBeenHere method.
		
		if (turn == 0){ // process left turn
			if (direction != 0){
				direction--;
			} else {
				direction = 3;
			}
		} else{ // process right turn
			if (direction != 3){
				direction++;
			} else{
				direction = 0;
			}
		}
	}
	
	static void updateCoordinates(){
		// This is practicing knowing where it is in case I can implement it
		// better.  Takes a value of time the motor was on, and uses that with
		// directional information to estimate location relative to its start.
		// Positive values are for right and up.
		
		if (direction == 0){ // left
			coordinates[0] -= currentTime;
		} else if (direction == 1){ // up
			coordinates[1] += currentTime;
		} else if (direction == 2){ // right
			coordinates[0] += currentTime;
		} else { // down
			coordinates[1] -= currentTime;
		}
	}
	
/* More complicated versions
	static void updateCoordinates(){
		// This takes the chrono timer value, looks at the direction and
		// previous coordinates, and adds an array within the big array to
		// represent positions for searching.
	}
	
	static void haveWeBeenHereBefore(){
		// This will allow the thing to remember better.
	}
 */
	
	// Ending: finish, passivateRCX
	static void finish() throws InterruptedException{
		TextLCD.print("Done!");
		Motor.A.setPower(6);
		Motor.B.setPower(6);
		Motor.A.forward();
		Motor.C.backward();
		Thread.sleep(3000);
		Motor.A.backward();
		Motor.C.forward();
		Thread.sleep(3000);
		
		completeStop(false);
		passivateRCX();
	}
	
	static void passivateRCX(){
		for (int i = 0; i <= 2; i++){
			Sensor.SENSORS[i].passivate();
		}
	}
}