//Importering av klasser som brukes
import lejos.nxt.*;
import java.util.Random;
import lejos.robotics.navigation.TachoPilot;

public class Sputnik
{
	public static void main(String[]args) throws Exception
	{
		//Oppretter nye objekt for sensorene
		UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);
		TouchSensor kant1 = new TouchSensor(SensorPort.S1);
		TouchSensor kant2 = new TouchSensor(SensorPort.S4);

		TachoPilot pilotLoft = new TachoPilot(46.6f, 46.6f, 10, Motor.B, Motor.B, false);

		//Oppretter et random objekt
		Random rnd = new Random();

		//Variabel for random sving retning
		int svingGrader = 0;
		int svingRetning = 0;
		//int kjøreDistanse = 0;

		//Teller for antall hindringer den har truffet
		int teller = 0;

		//LCD output
		LCD.clear();
		LCD.drawString("Hei, jeg heter:", 0, 0);
		LCD.drawString("Sputnik", 0, 1);
		LCD.drawString("Avstand: ", 0, 3);
		LCD.drawInt(sonic.getDistance(), 9, 3);
		LCD.refresh();

		//Senker den ned
		pilotLoft.setSpeed(20);
		pilotLoft.travel(-35);
		pilotLoft.stop();
		Thread.sleep(100);

		TachoPilot pilot = new TachoPilot(46.6f, 46.6f, 320, Motor.C, Motor.A, false);

		//Reduserer rykk og napp
		Motor.A.smoothAcceleration(true);
		//Motor.B.smoothAcceleration(true);
		Motor.C.smoothAcceleration(true);
		//Setter farten til Sputnik
		pilot.setSpeed(500);
		pilot.forward();

		while(teller != 10)
		{
			//Finner en radom verdi for hvor mye den skal
			//svinge og hvilken retning den skal svinge
			svingGrader = (rnd.nextInt(150) + 30);
			svingRetning = rnd.nextInt(11);

			//kjøreDistanse = (rnd.nextInt(20) * 100)
			//Slår ut om den kommer for nærme noe
			if(sonic.getDistance() < 20)
			{
				//if sjekker som avgjør hvor hvilken retning
				//den skal svinge eller om den bare skal rygge

				//Venstresving
				if(svingRetning < 5)
				{
					teller++;
					pilot.rotate(svingGrader);
					Thread.sleep(100);
					pilot.forward();
				}
				//Høyresving
				else if(svingRetning > 5)
				{
					teller++;
					pilot.rotate(-svingGrader);
					Thread.sleep(100);
					pilot.forward();
				}
				//Rygger, liten sjanse for at dette inntreffer
				else if(svingRetning == 5)
				{
					teller++;
					pilot.backward();
					Thread.sleep(1000);
					pilot.forward();
				}

				Thread.sleep(100);
			}

			//If setninger som tar for seg hva den skal gjøre når
			//touchsensoren(n) blir utløst. Håndterer bordkanter
			if(kant1.isPressed() == false)
			{
				teller++;
				pilot.travel(-150);
				pilot.rotate(svingGrader);
				pilot.forward();
			}
			else if(kant2.isPressed() == false)
			{
				teller++;
				pilot.travel(-150);
				pilot.rotate(-svingGrader);
				pilot.forward();
			}
		} //Avlutter løkken

		//Når den er ferdig med jobben
		//Løfter roboten seg i hvilestilling
		pilot.stop();
		pilotLoft.setSpeed(20);
		pilotLoft.travel(45);
		Motor.B.lock(60);
	}
}