package mindfinder;

import lejos.nxt.ColorLightSensor;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorConstants;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.ColorSensor;
import lejos.nxt.remote.NXTCommand;
import lejos.nxt.remote.RemoteMotor;
import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTConnector;
import lejos.pc.comm.NXTInfo;
import lejos.robotics.Colors.Color;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;

public class Controller {
	
	private Pilot pilot;
	
	private UltrasonicSensor uss;
	private ColorLightSensor cls;
	
	private NXTConnector conn;
	private NXTComm comm;
	
	private static final RemoteMotor motorLeft = Motor.A;
	private static final RemoteMotor motorRight = Motor.B; 
	private static final RemoteMotor motorSpecial = Motor.C; 
	
	
	public Controller() {
		
		pilot = new TachoPilot(2.1f, 4.4f, motorLeft, motorRight, false);
		
		
		uss = new UltrasonicSensor(SensorPort.S2);
		cls = new ColorLightSensor(SensorPort.S3, SensorConstants.MODE_CELSIUS);
		
		motorSpecial.setSpeed(100);
	}
	
	public NXTInfo connect(){
		conn = new NXTConnector();
		boolean connected = conn.connectTo();
		System.out.println("Connected: " + connected);
		NXTInfo[] infos = conn.getNXTInfos();
		System.out.println(infos[0].deviceAddress);
		comm = conn.getNXTComm();
		NXTCommand.getSingleton().setNXTComm(comm);
		
		if(infos.length > 0){
			return infos[0];
		}
		return null;
	}
	
	public void stop(){
		pilot.stop();
		Motor.C.stop();
		
	}
	
	public void forward(){
		pilot.forward();		
	}
	
	public void backward(){
		pilot.backward();
	}
	
	public void left(){
		motorRight.forward();
		motorLeft.backward();
	}
	
	public void right(){
		motorRight.backward();
		motorLeft.forward();
	}
	
	public void forwardLeft(){
		resetSpeed();
		int speedRight = motorRight.getSpeed();
		int speedLeft = speedRight / 2;
		motorLeft.setSpeed(speedLeft);
		
		motorRight.forward();
		motorLeft.forward();
		resetSpeed();
	}
	
	public void forwardRight(){
		resetSpeed();
		int speedLeft = motorLeft.getSpeed();
		int speedRight = speedLeft / 2;
		motorRight.setSpeed(speedRight);
		
		motorRight.forward();
		motorLeft.forward();
		resetSpeed();
	}
	
	public void backwardLeft(){
		resetSpeed();
		int speedRight = motorRight.getSpeed();
		int speedLeft = speedRight / 2;
		motorLeft.setSpeed(speedLeft);

		motorLeft.backward();
		motorRight.backward();
		resetSpeed();
	}
	
	public void backwardRight(){
		
		resetSpeed();
		int speedLeft = motorLeft.getSpeed();
		int speedRight = speedLeft / 2;
		motorRight.setSpeed(speedRight);
		
		motorRight.backward();
		motorLeft.backward();
		resetSpeed();
	}
	
	public void resetSpeed(){
		motorLeft.setSpeed(700);
		motorRight.setSpeed(700);
	}
	
	
	public void headRight(){
		motorSpecial.forward();
	}
	
	public void headLeft(){
		motorSpecial.backward();
	}
	
	public void headStop(){
		motorSpecial.stop();
	}
	
	public String getUltrasonicDistance(){
		return Integer.toString(uss.getDistance());
	}
	
	public String getLightValues(){
		int blue = cls.getBlueComponent();
		
		
//		String color = ls.getFloodlight().toString();
//		String lightValue = Integer.toString(ls.getLightValue());
//		String readValue = Integer.toString(ls.readValue());
		String res = "Lichtwert: " + blue;
		return res;
	}
	
	public void setFloodLight(){
		cls.setFloodlight(true);
	}
	
	public void calibrateHigh(){
		cls.calibrateHigh();
	}
	
	public void calibrateLow(){
		cls.calibrateLow();
	}
	
	public float getPilotMaxSpeed(){
		return pilot.getMoveMaxSpeed();
	}
	
	public void setPilotSpeed(float speed){
		pilot.setMoveSpeed(speed);
	}
	
	public void playBeep(){
		Sound.playSoundFile("r2d2_1.wav");
	}
	
}