package atrox;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.SimpleNavigator;



//////////////////////////////////
/////////////////////////



public class MyFirstProgram {
	  Pilot pilot;
	  SimpleNavigator navi;
	LightSensor light1;
	LightSensor light2;
	LightSensor light3;

	/**
	 * This is the Entrypoint for the program, do not change it.
	 */
	public static void main(String[] args) {
		MyFirstProgram hw2=new MyFirstProgram();
		try {
			hw2.run();
		} catch (InterruptedException e) {} 
	}
	
	/**
	 * Here you do initialisation, like defining Sensors
	 */
	public MyFirstProgram() {

		light1=new LightSensor(SensorPort.S1);
		light2=new LightSensor(SensorPort.S2);
		
		light3=new LightSensor(SensorPort.S3);
		pilot = new lejos.robotics.navigation.TachoPilot(5.62f, 16.4f, Motor.A, Motor.B,true);
		navi = new SimpleNavigator(pilot);
		navi.setPosition(0, 0, 90);
	}
	
	/**
	 * Do your work here
	 */
	public void run() throws InterruptedException {

		
		int i[] = new int[3];  // some variable
		int lastseenblack=0;
	
		while (Button.ENTER.isPressed()); // wait until released
		
		boolean dec[] = new boolean[3]; 
		// repeat while the ENTER Button is NOT pressed
		while ( !Button.ENTER.isPressed() ) {
			if (Button.LEFT.isPressed()) { // when the left button is pressed, decrement i
				i[1]=light1.getLightValue();
				
			}
			if (Button.RIGHT.isPressed()) { // when the right button is pressed, increase i
				i[1]=light1.getLightValue();
			}	
			i[0]=light1.getLightValue();//right
			i[1]=light2.getLightValue();//middle
			i[2]=light3.getLightValue();//left
			
			for(int j=0;j<3;j++)
			{
				if(i[j]>43)
				{
					dec[j]=false;
				}
				else
					dec[j]=true;
			}
			
			
		//	pilot.forward();
			//pilot.steer(-5,45); 
	
		/*	
			if(dec[1])
			{
				Motor.A.setSpeed(500);
				Motor.B.setSpeed(500);
				Motor.A.backward();
				Motor.B.backward();
			}
			else if(dec[0])
			{
				Motor.A.setSpeed(300);
				Motor.B.setSpeed(200);
				Motor.A.backward();
				Motor.B.forward();
			}
			else*/ 
			
			LCD.drawString(i[2]+" ", 0, 0);
			
		/*	if (dec[2] && dec[0]) {
				// intersection
				pilot.arc(0,-10,false); 
				
			} else */
			if(dec[2])
			{
				// dark
				
				double heading=Math.toRadians(navi.getHeading());
				
				//LCD.drawString(heading+" ", 1, 1);
				
				int x=(int)Math.round((navi.getX() +Math.cos(heading)*15 )/3);
				int y=(int)Math.round((navi.getY() +Math.sin(heading)*15 )/3) -5 ;
				
				LCD.setPixel(1, LCD.DISPLAY_WIDTH/2-x, y);
				
				if (lastseenblack>0) {
					pilot.arc(-5,-20,false);
				} else {
					pilot.arc(10,10,false); 
				}
			//	Motor.A.setSpeed(70);
			//	Motor.B.setSpeed(100);
			//	Motor.A.forward();
			//	Motor.B.backward();
				
				//Thread.sleep(500); 
				lastseenblack++;
			}
			else{
				// white
				lastseenblack=0;
				
				//pilot.arc(-8);
				//pilot.steer(-200);
				pilot.setTurnSpeed(37);
     	    	pilot.rotate(-20,true);
				
				
     	    //	Motor.A.setSpeed(70);
			//	Motor.B.setSpeed(1);
			//	Motor.A.forward();
			//	Motor.B.backward();
			}		
									
			/*
			else if(dec[2] && dec[1])
			{
				Motor.A.setSpeed(300);
				Motor.B.setSpeed(400);
				Motor.A.backward();
				Motor.B.backward();
			}
			else if(dec[0] && dec[1])
			{
				Motor.A.setSpeed(400);
				Motor.B.setSpeed(300);
				Motor.A.backward();
				Motor.B.backward();
			}*/
			
		
		
			
			//LCD.clearDisplay();
			//LCD.drawInt(i[0],8,1);
			//LCD.drawInt(i[1],1,1);
			//LCD.drawInt(i[2],14,1);
			

			
			//Thread.sleep(100); // sleep 250 ms = 1/4 sec
			
		} // end of while
	
	}
	
}