import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.NXTMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.SensorMode;
import lejos.utility.Delay;

public class LightFollower {

	public static void main(String[] args) {
		// TODO Auto-generated method stub

		EV3ColorSensor colorSensor = new EV3ColorSensor(SensorPort.S1);
		SensorMode mode = colorSensor.getAmbientMode();

		Sound.twoBeeps();
		LCD.drawString("Light", 0, 0);
		Button.ESCAPE.waitForPressAndRelease();
		float[] sampleLight = new float[1];
		mode.fetchSample(sampleLight, 0);
	    int sampleLightInt = new Float(sampleLight[0] * 100).intValue(); 
		LCD.drawInt(sampleLightInt, 2, 2);
		Delay.msDelay(5000);
		
		Sound.twoBeeps();
		LCD.drawString("Dark", 4, 4);
		Button.ESCAPE.waitForPressAndRelease();
		float[] sampleDark = new float[1];
		mode.fetchSample(sampleDark, 0);
		int sampleDarkInt = new Float(sampleDark[0] * 100).intValue();
		LCD.drawInt(sampleDarkInt, 6, 6);
		Delay.msDelay(5000);

		LCD.clear();
		
		int defaultPower = 20;
		int multiplyingFactor = 50 ;

		NXTMotor largeMotorB = new NXTMotor(MotorPort.B);
		NXTMotor largeMotorC = new NXTMotor(MotorPort.C);
		float[] color = new float[1];

		while (!Button.ESCAPE.isPressed()) {

			float avgLight = (sampleDarkInt + sampleLightInt) / 2;
			mode.fetchSample(color, 0);
			int colorInt = new Float(color[0] * 100).intValue(); 
		    LCD.drawInt(colorInt, 6, 6);
		    
			float cSpeed = defaultPower + multiplyingFactor
					* (avgLight - colorInt) / (sampleLightInt - sampleDarkInt);
			largeMotorC.setPower(new Float(cSpeed).intValue());
			largeMotorC.forward();

			float bSpeed = defaultPower - multiplyingFactor
					* (avgLight - colorInt) / (sampleLightInt - sampleDarkInt);
			largeMotorB.setPower(new Float(bSpeed).intValue());
			largeMotorB.forward();
		}

	}

}