package atrox;
import lejos.nxt.*;

public class Maze {

	LightSensor left = new LightSensor(SensorPort.S1);
	LightSensor mid = new LightSensor(SensorPort.S2);
	LightSensor right = new LightSensor(SensorPort.S3);
	
	int numberOfSensors;
	
	int white;
	int black;
	
	public static void main(String[] args) {
		Maze maze = new Maze();
		try {
			maze.run();
		} catch (InterruptedException e) {} 
	}
	
	public Maze() {
		/* ... */
	}
	
	public void run() throws InterruptedException{
		numberOfSensors = 1;
		
		callibrate();
		followMaze();
	}
	
	public void callibrate(){
		int first = 0;
		int second = 0;
		
		while (!Button.ESCAPE.isPressed()){
			first = left.readValue();
			second = right.readValue();
			
			LCD.clearDisplay();
			System.out.println("White");
			System.out.println("Left:" + first);
			System.out.println("Right" + second);
			
			try {
				Thread.sleep(100);
			} catch (InterruptedException e) {}
		}
		
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {}
		
		white = second;
		
		while (!Button.ESCAPE.isPressed()){
			first = left.readValue();
			second = right.readValue();
			
			LCD.clearDisplay();
			System.out.println("Black");
			System.out.println("Left:" + first);
			System.out.println("Right" + second);
			
			try {
				Thread.sleep(100);
			} catch (InterruptedException e) {}
		}
		
		black = second;
	}
	
	public void followLine(){
		int rightValue = right.readValue();
		int leftValue = left.readValue();
		
		if(numberOfSensors == 1){
			try {
				Thread.sleep(500);
			} catch (InterruptedException e) {}
			
			rightValue = right.readValue();
			leftValue = left.readValue();
		
			LCD.clearDisplay();
			System.out.println("Following line");
			System.out.println("Left:" + leftValue);
			System.out.println("Right" + rightValue);
			
			int value = left.readValue();
			
			if(value >= white){
				Motor.C.backward();
				
				System.out.println("Steer left");
				
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {}
				
				Motor.C.forward();
			}
			else if(value <= (black + 5)){
				Motor.A.backward();
				
				System.out.println("Steer right");
				
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {}
				
				Motor.A.forward();
			}
		}
	}
	
	public void followMaze(){
		Motor.A.forward();
		Motor.C.forward();
		
		Motor.A.setSpeed(50);
		Motor.C.setSpeed(50);
		
		while(true){
			int rightValue = right.readValue();
			int leftValue = left.readValue();
			
			LCD.clearDisplay();

			if(rightValue <= (black + 5)){
				Motor.A.backward();
				
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {}
				
				while(left.readValue() >= white){
					leftValue = left.readValue();
					rightValue = right.readValue();
					
					LCD.clearDisplay();	
					System.out.println("Turning right");
					System.out.println("Left:" + leftValue);
					System.out.println("Right" + rightValue);
					
					try {
						Thread.sleep(500);
					} catch (InterruptedException e) {}
				}
				
				Motor.A.forward();
			}
			else if(leftValue <= (black + 5) && rightValue >= white){
				followLine();
			}
			else if(leftValue >= white && rightValue >= white){
				LCD.clearDisplay();
				System.out.println("Dead end turn");
				System.out.println("Left:" + leftValue);
				System.out.println("Right" + rightValue);
				
				Motor.C.backward();
				
				try {
					Thread.sleep(1000);
				} catch (InterruptedException e) {}
				
				Motor.C.forward();
			}
		}
	}
}