package week7;

import lejos.nxt.BasicMotorPort;
import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;

public class vehicle2 {

	private static int max = 1023;
	private static LightSensor ls1 = new LightSensor(SensorPort.S2);
	private static LightSensor ls2 = new LightSensor(SensorPort.S3);

	private static boolean ab = true; // true = a, false = b;

	public static void main(String [] args) throws Exception
    {

		float maxLight = 10000;
		float minLight = 0;
		float alpha = 0.01F;

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

			/*float lightValue1 = ls1.readValue();
			float lightValue2 = ls2.readValue();*/

			// Inhibitory
			/*lightValue1 = 100-lightValue1;
			lightValue2 = 100-lightValue2;*/

			float lightValue1 = ls1.readNormalizedValue();
			float lightValue2 = ls2.readNormalizedValue();

			if(lightValue1 < maxLight*2) {
				maxLight = (maxLight * (1-alpha)) + lightValue1*alpha;
			}
			if(lightValue2 < maxLight*2) {
				maxLight = (maxLight * (1-alpha)) + lightValue2*alpha;
			}
			if(lightValue1 > minLight/2) {
				minLight = (minLight * (1-alpha)) + lightValue1*alpha;
			}
			if(lightValue2 > minLight/2) {
				minLight = (minLight * (1-alpha)) + lightValue2*alpha;
			}

			lightValue1 = normalize(lightValue1, maxLight, minLight);
			lightValue2 = normalize(lightValue2, maxLight, minLight);

			// Inhibitor
			/*lightValue1 = 100 - lightValue1;
			lightValue2 = 100 - lightValue2;*/


			if(ab) {
				MotorPort.A.controlMotor((int) lightValue1, BasicMotorPort.FORWARD);
		        MotorPort.C.controlMotor((int) lightValue2, BasicMotorPort.FORWARD);
			}
			else {
				MotorPort.A.controlMotor((int) lightValue2, BasicMotorPort.FORWARD);
		        MotorPort.C.controlMotor((int) lightValue1, BasicMotorPort.FORWARD);
			}

		}
    }

	public static float normalize(float lightValue1, float maxLight, float minLight) {

		float output = 100 - ((lightValue1 - maxLight) * 100) / (minLight - maxLight);
		if(output > 100) {
			output = 100;
		}
		if(output < 0) {
			output = 0;
		}
		return output;
	}
}