import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;


/**
 * This simple NXT program changes drive state based on the current color value
 * 
 * @author Will Gorman
 */
public class IPhoneBot {
	
	// the main light sensor for driving
	
	static final LightSensor center_light = new LightSensor(SensorPort.S1, true);

	public static void main(String args[]) throws Exception {
		
		Thread.sleep(1000);
		
		// take a measurement, expecting white
		
		LCD.clear();
		LCD.drawString("cal white...", 0, 0);
		LCD.refresh();
		
		Button.ENTER.waitForPressAndRelease();
		
		Thread.sleep(1000);
		
		int measure = center_light.readNormalizedValue();
		center_light.calibrateHigh();

		// take a measurement, expecting black
		
		LCD.clear();
		LCD.drawString("cal black...", 0, 0);
		LCD.drawInt(measure, 0, 1);
		LCD.refresh();
		Button.ENTER.waitForPressAndRelease();
		Thread.sleep(1000);
		
		int next_measure = center_light.readNormalizedValue();
		center_light.calibrateLow();
		
		LCD.clear();
		LCD.drawString("init complete", 0, 0);
		LCD.drawInt(next_measure, 0, 1);
		LCD.drawInt(measure, 0,2);
		LCD.refresh();
		
		// 0 to 100 is now set, go into regular mode
		
		Thread.sleep(5000);
		
		// 5 values, 
		int oldstate = -1;
		int currstate = 0;
		int val = 0;
		int rawval = 0;
		
		// main loop
		
		while (true) {
			// normalize to 5 bins, 0 thru 4
			rawval = center_light.readNormalizedValue();
			val = center_light.readValue();
			currstate = (val * 5) / 100;
			if (currstate != oldstate) {
				oldstate = currstate;
				int left = 0;
				int right = 0;
				
				// depending on the current state,
				// drive in the specified direction
				
				if (currstate <= 0) {
						left = 0;
						right = 0;
				} else if (currstate == 1) {
						left = -1;
						right = 1;
				} else if (currstate == 2) {
						left = 1;
						right = -1;
				} else if (currstate == 3) {
						left = 1;
						right = 1;
				} else if (currstate >= 4) {
						left = -1;
						right = -1;
				}
		
				LCD.clear();
				LCD.drawInt(rawval, 0, 0);
				LCD.drawInt(val, 0, 1);
				LCD.drawInt(currstate, 0, 2);
				LCD.drawInt(left, 0, 3);
				LCD.drawInt(right, 0, 4);
				LCD.refresh();
				
				// left motor is A
				
				if (left == 0) {
					Motor.A.stop();
				} else if (left == 1) {
					Motor.A.forward();
				} else if (left == -1) {
					Motor.A.backward();
				}
				
				// right motor is B
				
				if (right == 0) {
					Motor.C.stop();
				} else if (right == 1) {
					Motor.C.forward();
				} else if (right == -1) {
					Motor.C.backward();
				}
			}
		}
	}
}