/*
 * This program version begins with stepping through sensor values, and tries
 * the movement options.
 * 
 *
 * 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.
 *
 * *******************
 * Julie Powers-Boyle
 * 20060220 Mon, Programming WS
 * $ID
 * *******************
 */

import josx.platform.rcx.*;

public class BetterNav implements SensorConstants{
    
    // different values in case there are differences among sensors
    static int backgroundValues[] = {0,0,0};
    static int trackValues[] = {0,0,0};
    
    public static void main (String[] args) throws InterruptedException{
	
	initialize();
	getLightValues();
	
	//Don't run away until someone says to do so
	TextLCD.print("RUN?");
	Button.RUN.waitForPressAndRelease();
	
	// Test out the moves
	moveForward();
	Thread.sleep(3000);
	completeStop();
	Thread.sleep(250);
	
	turnRight();
	turnLeft(); // This should result in the middle sensor still at the pivot
	
	moveBackward();
	Thread.sleep(3000);
	completeStop();
	Button.RUN.waitForPressAndRelease();
	
    }  // closing main

    //decomposed methods:
    
    static void initialize(){
	
	for (int i = 0; i <= 2; i++){
	    Sensor.SENSORS[i].setTypeAndMode(SENSOR_TYPE_LIGHT, SENSOR_MODE_PCT);
	    Sensor.SENSORS[i].activate();
	}
	Motor.A.setPower(3);  // just to start
	Motor.C.setPower(3);
    }
    
    static void getLightValues() throws InterruptedException{
	
	// I'm making this much more complicated than I probably need to later,
	// because different-voltaged sensors might read the same values.
	
	// This idea adapted from Jim's LineWalker.java
	String displayThis;
	int i;
	
	TextLCD.print("W 0");
	for (i = 0; i <= 2; i++){
	    Button.PRGM.waitForPressAndRelease();
	    backgroundValues[i] = Sensor.SENSORS[i].readValue();
	    displayThis = "W" + Integer.toString(i+1) + " " + Integer.toString(backgroundValues[i]);
	    TextLCD.print(displayThis); //let's watch bugs
	}
	Thread.sleep(1000);
	
	// yes, I could probably nest these loops
	TextLCD.print("B 0");
	for (i = 0; i <= 2; i++){
	    Button.VIEW.waitForPressAndRelease();
	    trackValues[i] = Sensor.SENSORS[i].readValue();
	    displayThis = "B" + Integer.toString(i+1) + " " + Integer.toString(trackValues[i]);
	    TextLCD.print(displayThis); //let's watch bugs
	}
	Thread.sleep(1000);
    }
    
    static void moveForward(){
	Motor.A.forward();
	Motor.C.forward();
    }
    
    static void moveBackward(){
	Motor.A.backward();
	Motor.C.backward();
    }
    
    static void completeStop() throws InterruptedException{
	Motor.A.stop();
	Motor.C.stop();
	Thread.sleep(500);
    }
    
    static void turnRight() throws InterruptedException{
	Motor.A.forward();
	Motor.C.backward();
	Thread.sleep(1000);  // this needs to be calibrated
	TextLCD.print("SPLAT");
	completeStop();
    }
    
    static void turnLeft() throws InterruptedException{
	Motor.A.backward();
	Motor.C.forward();
	Thread.sleep(1000);  // this needs calibration
	TextLCD.print("heh");
	completeStop();
    }
    
    static void backOutFromDeadEnd(){
	// If the robot finds itself in a dead end, its navigation plan is a
	// little bit different from its usual one.
    }
}