/*
 * 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.
 *
 * If I'm not using it right now, I'm commenting it out.
 * 
 * *******************
 * Julie Powers-Boyle
 * 20060228 Tues, 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 choiceLog[][] = new int[20][4]; // can I add to its size?
	static int timeLog[] = new int[20]; // and to this?
	static int stepNumber; // this serves as an index number for arrays
	static int currentTime;
	static boolean different; // switched by SensorListener
	static Timer chrono = new Timer(10, new StopTimer());
	static Random random = new Random(3);
	
	public static void main (String args[]) throws InterruptedException{
	
		activateRCX();
		initializeVariables();
		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, initializeVariables, 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(1);
		Motor.C.setPower(1);
	}
	
	static void initializeVariables() {
		choiceLog[0][0] = 0;
		choiceLog[0][1] = 1;
		choiceLog[0][2] = 0;
		choiceLog[0][3] = 1;
		timeLog[0] = 0;
		stepNumber = 1;
		different = false;
	}
	
	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();
			while (different == false){ // bide time between sensor events
			 Thread.sleep(9);
			}
			completeStop(true);
			if (areWeOnTrack() == false){
			} else {
				updateChoiceLogSensors();
				while (newWayToGo() == false){
					backOutFromDeadEnd();
				}
				chooseDirection();
				turnCorrectly();
				for (int i = 0; i < 3; i++) { // break for finish
					if (choiceLog[stepNumber-1][i] == 3) {
						return;
					}
				}
				stepNumber++;
				TextLCD.print(Integer.toString(stepNumber));
				Button.RUN.waitForPressAndRelease();
				
				TextLCD.print("end");
				Button.RUN.waitForPressAndRelease();
			}
			
		}
	}
	
	// Movement: moveForward, moveBackward, turnLeft, turnRight, completeStop
	static void moveForward(){
		activateTimer();
		Motor.A.forward();
		Motor.C.forward();
		different = false; // reset for watching
		TextLCD.print("new");
	}
	
	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(1200);	 // this is not consistent in distance
		TextLCD.print("heh");
		completeStop(false);
	}
	
	static void turnRight() throws InterruptedException{
		Motor.A.forward();
		Motor.C.backward();
		Thread.sleep(1200);	 // this will get it within 20 degrees ...
		TextLCD.print("SPLAT");
		completeStop(false);
	}
	
	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();
		}
		TextLCD.print("stop");
		Thread.sleep(3000); // extra-long pause for human intervention
	}
	
	// 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).

		TextLCD.print("choos");
		try{
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
		}
		
		int someNum = Math.round(Math.abs(random.nextInt())); // an int under 3
		//double interim = Math.random()*2;
		//int someNum = Math.round(interim);
		
		TextLCD.print(Integer.toString(someNum));
		try{
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
		}
		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("badir");
		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.
		
		TextLCD.print("end");
		Button.RUN.waitForPressAndRelease();
		
		TextLCD.print(Integer.toString(timeLog[stepNumber]));
		Button.RUN.waitForPressAndRelease();
		
		moveBackward();
		Thread.sleep(timeLog[stepNumber]);
		completeStop(false);
		turnOpposite();
		stepNumber--;
		TextLCD.print(Integer.toString(stepNumber));
		Button.RUN.waitForPressAndRelease();
		
		choiceLog[stepNumber][(choiceLog[stepNumber][3])] = 2; // bad route
		
		TextLCD.print("log");
		Button.RUN.waitForPressAndRelease();
		
		TextLCD.print(Integer.toString(choiceLog[stepNumber][0]));
		Button.RUN.waitForPressAndRelease();
		
		TextLCD.print(Integer.toString(choiceLog[stepNumber][1]));
		Button.RUN.waitForPressAndRelease();
		
		TextLCD.print(Integer.toString(choiceLog[stepNumber][2]));
		Button.RUN.waitForPressAndRelease();
		
		TextLCD.print(Integer.toString(choiceLog[stepNumber][3]));
		Button.RUN.waitForPressAndRelease();
	}
	
	// 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.
		
		TextLCD.print("check");
		Button.RUN.waitForPressAndRelease();
		if (timeLog[stepNumber] < 100) {
			TextLCD.print("Off");
			Sound.playTone(110, 20);
			Button.RUN.waitForPressAndRelease(); // put it on last intersection
			
			TextLCD.print(Integer.toString(timeLog[stepNumber]));
			Button.RUN.waitForPressAndRelease();
			LCD.refresh();
			return false;
		}
		return true;
	}
	
	static boolean newWayToGo(){
		// Tests if this is a dead end.
		
		TextLCD.print("route");
		try{
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
		}
		for (int i = 0; i <= 2; i++){
			if (choiceLog[stepNumber][i] == 1) {
				TextLCD.print("yes");
				try{
					Button.RUN.waitForPressAndRelease();
				}catch (InterruptedException e) {
				}
				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 reports back.
		
		TextLCD.print("color");
		Button.RUN.waitForPressAndRelease();
		
		int lightness = Sensor.SENSORS[whichSensor].readValue();
		
		if (Math.abs(lightness - backgroundValues[whichSensor]) < 3) {
			return 0;
		} else if (Math.abs(lightness - trackValues[whichSensor]) < 3) {
			return 1;
		} else if (Math.abs(lightness - finishValues[whichSensor]) < 3) {
			return 3;
		} else {
			TextLCD.print("blind");
			Button.RUN.waitForPressAndRelease();
			
			System.exit(1);
			return 4; // Does exit not work, then?
		}
	}
	
	static void turnCorrectly() throws InterruptedException{
		TextLCD.print("turn");
		Button.RUN.waitForPressAndRelease();
		
		if (choiceLog[stepNumber][3] == 0){
			turnLeft();
		} else if (choiceLog[stepNumber][3] == 2) {
			turnRight();
		}
		TextLCD.print(Integer.toString(choiceLog[stepNumber][3]));
		Button.RUN.waitForPressAndRelease();
	}
	
	static void turnOpposite() throws InterruptedException {
		TextLCD.print("nrut");
		Button.RUN.waitForPressAndRelease();
		if (choiceLog[stepNumber][3] == 0){
			turnRight();
		} else if (choiceLog[stepNumber][3] == 2) {
			turnLeft();
		}
		TextLCD.print(Integer.toString(choiceLog[stepNumber][3]));
		Button.RUN.waitForPressAndRelease();
	}
	
	// Write: updateChoiceLogSensors, updateTimeLog, updateDirection,
		// updateCoordinates
	static void updateChoiceLogSensors() throws InterruptedException {
		// Enlightens this array to the most recent sensor values.
		
		TextLCD.print("array");
		Button.RUN.waitForPressAndRelease();
		
		for (int i = 0; i <= 2; i++) {
			choiceLog[stepNumber][i] = whatColorIsIt(i);
			
			TextLCD.print("ch " + Integer.toString(i));
			Button.RUN.waitForPressAndRelease();
			TextLCD.print(Integer.toString(choiceLog[stepNumber][i]));
			Button.RUN.waitForPressAndRelease();
		}
	}
	
	static void updateTimeLog(){
		// Whenever it stops, it will put for how many hundredths of seconds the
		// motor had been on then.
		
		timeLog[stepNumber] = currentTime;
		try{
			TextLCD.print("time");
			Button.RUN.waitForPressAndRelease();
			TextLCD.print(Integer.toString(timeLog[stepNumber]));
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
		}

	}
	
	// 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();
		}
	}
}