/*
 * This program version will attempt navigation through a tracked environment.
 * 
 * Having had a bunch of difficult-to-debug problems with memory in MazeNav
 * this is a very stripped down version.  In this one, the brick will have 
 * severe executive function problems which causes it to be very present-
 * oriented.  In fact, it is exclusively concerned with the here and now, and
 * makes no attempt to use any sort of problem-solving strategy.  Rather, it is
 * a toddler first figuring out how to interact with an environment.
 * 
 * User information elsewhere.
 * 
 * *******************
 * Julie Powers-Boyle
 * 20060301, Programming WS
 * $Id
 * *******************
 */

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

public class PrimitiveNav 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 sensoryState[] = {0,0,0};
	static int directionalConviction = 1;
	static int lightness = 0;
	static int someNum = 0;
	static boolean goingBackward = false;
	static boolean different = false; // switched by SensorListener
	
	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, 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 PrimitiveWatcher());
		}
		Motor.A.setPower(1);
		Motor.C.setPower(1);
	}
	
	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.
		
		moveForward();
		
		while (true){
			Thread.sleep(500); // so it gets past its current lines
			different = false; // reset for watching
			
			while (different == false){ // bide time between sensor events
			 Thread.sleep(15);
			}
			
			completeStop();
			updateSensoryState();
			
			if (aWayToGo() == false){
				TextLCD.print("again");
				Button.RUN.waitForPressAndRelease();
				moveBackward();
			} else {
				chooseDirection();
				turnAccordingly();
				TextLCD.print("end");
				Button.RUN.waitForPressAndRelease();
				moveForward();
			}
			
			for (int i = 0; i <= 2; i++) { // break for finish
				if (sensoryState[i] == 3) {
					return;
				}
			}
			
		}
	}
	
	// Movement: moveForward, moveBackward, turnLeft, turnRight, completeStop
	static void moveForward(){
		goingBackward = false;
		Motor.A.forward();
		Motor.C.forward();
		TextLCD.print("new");
	}
	
	static void moveBackward(){
		goingBackward = true;
		Motor.A.backward();
		Motor.C.backward();
	}
	
	static void turnLeft() throws InterruptedException{
		Motor.A.backward();
		Motor.C.forward();
		Thread.sleep(1000);	 // clumsy, clumsy
		completeStop();
	}
	
	static void turnRight() throws InterruptedException{
		Motor.A.forward();
		Motor.C.backward();
		Thread.sleep(1000);	 // look at that undeveloped motor coordination
		completeStop();
	}
	
	static void completeStop() throws InterruptedException{
		Motor.A.stop();
		Motor.C.stop();
		TextLCD.print("stop");
		Thread.sleep(2200); // long pause for parental guidance
	}
	
	// Sensors: (now in PrimitiveWatcher)
	
	// Other confused stuff
	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) {
		}
		
		someNum = (int)Math.floor(3*Math.random());
		
		LCD.showNumber(someNum);
		try{
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
		}
		for (int i = 0; i < 3; i++){
			if(sensoryState[someNum] == 1){
				directionalConviction = someNum;
				LCD.showNumber(someNum);
				try{
					Button.RUN.waitForPressAndRelease();
				}catch (InterruptedException e) {
				}
				return;
			} else if (someNum != 2){
				someNum++;
			} else {
				someNum = 0;
			}
		}
		// This indicates a blind faith in the maze-makers that there is a
		// solution, and in the robot that it is not blind or clumsy.
		TextLCD.print("badir");
		try{
			Button.RUN.waitForPressAndRelease();
		}catch (InterruptedException e) {
			System.exit(1);
		}
	}
	
	static boolean aWayToGo(){
		// 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 (sensoryState[i] == 1) {
				TextLCD.print("yes");
				try{
					Button.RUN.waitForPressAndRelease();
				}catch (InterruptedException e) {
				}
				return true;
			}
		}
		
		TextLCD.print("dead");
		try {
			Button.RUN.waitForPressAndRelease();
		} catch (InterruptedException e){
		}
		return false; // if all sensors read yellow
	}
	
	static int whatColorIsIt(int whichSensor) throws InterruptedException {
		// This assimilates the current sensory experience with its pre-existing
		// schemata, and reports back.
		
		TextLCD.print("color");
		Button.RUN.waitForPressAndRelease();
		
		lightness = Sensor.SENSORS[whichSensor].readValue();
		
		LCD.showNumber(lightness);
		Button.RUN.waitForPressAndRelease();
		
		if (Math.abs(lightness - backgroundValues[whichSensor]) < 3) {
			return 0;
		} else if (Math.abs(lightness - trackValues[whichSensor]) < 4) {
			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 turnAccordingly() throws InterruptedException{
		TextLCD.print("turn");
		Button.RUN.waitForPressAndRelease();
		
		if (directionalConviction == 0){
			turnLeft();
		} else if (directionalConviction == 2) {
			turnRight();
		}
		LCD.showNumber(directionalConviction);
		Button.RUN.waitForPressAndRelease();
	}
	
	static void updateSensoryState() throws InterruptedException {
		// Enlightens this array to this slice of sensor values.
		
		TextLCD.print("array");
		Button.RUN.waitForPressAndRelease();
		
		for (int i = 0; i <= 2; i++) {
			sensoryState[i] = whatColorIsIt(i);
			
			TextLCD.print("arr " + Integer.toString(i));
			Button.RUN.waitForPressAndRelease();
			LCD.showNumber(sensoryState[i]);
			Button.RUN.waitForPressAndRelease();
		}
	}
	
	// Ending: finish, passivateRCX
	static void finish() throws InterruptedException{
		// Accordingly, it will behave like a toddler
		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();
		passivateRCX();
	}
	
	static void passivateRCX(){
		for (int i = 0; i <= 2; i++){
			Sensor.SENSORS[i].passivate();
		}
	}
}