import lejos.hardware.Button;
import lejos.hardware.LED;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.Motor;
import lejos.hardware.motor.NXTRegulatedMotor;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.hardware.sensor.MindsensorsDistanceSensor;
import lejos.hardware.sensor.SumoEyesSensor;
import lejos.robotics.SampleProvider;

public class WallSpoolFInder {

	private static final byte SUMO_NOTHING = 1;
	private static final byte SUMO_LEFT = 2;
	private static final byte SUMO_RIGHT = 3;
	private static final byte SUMO_CENTER = 4;

	public static NXTRegulatedMotor leftMotor = Motor.D;
	public static NXTRegulatedMotor rightMotor = Motor.B;
	public static NXTRegulatedMotor doorMotor = Motor.C;
	public static EV3UltrasonicSensor sonic = new EV3UltrasonicSensor(SensorPort.S1);
	public static MindsensorsDistanceSensor iRSensor = new MindsensorsDistanceSensor(SensorPort.S4);
	public static SumoEyesSensor eyes = new SumoEyesSensor(SensorPort.S2);

	public static EV3ColorSensor colorSensor = new EV3ColorSensor(SensorPort.S3);
	public static SampleProvider colorSampler = colorSensor.getRGBMode();

	public static float[] readings = new float[4];

	public static float[] sonarReadings = new float[sonic.getDistanceMode()
			.sampleSize()];
	public static float[] iRreadings = new float[1];
	static float[] colourReadings = new float[4];

	static int blackColourCount = 0;
	static int frontSensorDistance = 20;
	static int sideSensorDistance = 13;
	static int loopTimes = 4;

	public static void main(String[] args) throws Exception {
		eyes.setLongRange(true);

		byte sumoReading = getReading(eyes.getValue());

		doorMotor.rotateTo(75);
		Button.waitForAnyPress();

		leftMotor.setSpeed(300 * 1);
		rightMotor.setSpeed(300 * 1);

		startMovePot();
		leftMotor.stop(true);
		rightMotor.stop(true);
		//doorMotor.rotateTo(75);
		rightMotor.forward();
		leftMotor.backward();

		Thread.sleep(750);

		leftMotor.stop(true);
		rightMotor.stop(true);

	//	Button.waitForAnyPress();

		leftMotor.forward();
		rightMotor.forward();
		boolean flip  = false;


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

			colorSampler.fetchSample(colourReadings, 0);
			int r = (int) (colourReadings[0] * 1000);
			int g = (int) (colourReadings[1] * 1000);
			int b = (int) (colourReadings[2] * 1000);

			if((r < 45 && g < 45 && b < 45) && flip == false){
				flip = true;
				loopTimes--;
				LCD.drawString("Loop Value: " + loopTimes + "  ", 0, 6);
				LCD.drawString("FLIP: " + flip + "  ", 0, 7);

				Button.LEDPattern(3);
			}
			// else if(r < 10 && g < 10 && b < 10){
			//	 flip = true;
			//	 LCD.drawString("FLIP: " + flip + "  ", 0, 7);
			//	 Button.LEDPattern(0);
			// }
			 else if((r>45 || g>45 || b>45) && flip == true){
				 flip = false;
				 LCD.drawString("FLIP: " + flip + "  ", 0, 7);
				 Button.LEDPattern(0);
			 }



			sonic.getDistanceMode().fetchSample(sonarReadings, 0);
			float d = (int) (sonarReadings[0] * 1000) / 10f;

			int value = eyes.getValue();
			LCD.drawString("Raw Value: " + value + "  ", 0, 1);
			byte reading = getReading(value);



			iRSensor.fetchSample(iRreadings, 0);
			float di = iRreadings[0] * 10;

			// BACK END SEES SOMETHING

			if(loopTimes <=0){
				Thread.sleep(1000);
				leftMotor.stop(true);
				rightMotor.stop(true);
				return;
			}else
			if ((reading == SUMO_RIGHT ||reading == SUMO_CENTER || reading == SUMO_RIGHT)){
				//LCD.drawString("CENTER ", 0, 1);
				Thread.sleep(700);
				leftMotor.stop(true);
				rightMotor.stop(true);
				doorMotor.rotateTo(0);
				leftMotor.backward();
				rightMotor.backward();
				Thread.sleep(2200);
				rightMotor.forward();
				leftMotor.backward();
				Thread.sleep(1100);
				leftMotor.stop(true);
				rightMotor.stop(true);
				doorMotor.rotateTo(70);
				leftMotor.forward();
				rightMotor.forward();
				Thread.sleep(800);
			}else
				if (d < 10) {
					leftMotor.stop(true);
					rightMotor.stop(true);
					rightMotor.forward();
					leftMotor.backward();

					Thread.sleep(750);
					rightMotor.forward();
					leftMotor.forward();
				}else

			if (di > sideSensorDistance + 4) {
				leftMotor.setSpeed(300);
				rightMotor.setSpeed(175);
				Thread.sleep(400);
				leftMotor.setSpeed(300);
				rightMotor.setSpeed(300);
			}else if (di < sideSensorDistance) {
				rightMotor.setSpeed(300);
				leftMotor.setSpeed(175);
				Thread.sleep(400);
				leftMotor.setSpeed(300);
				rightMotor.setSpeed(300);
			}
		}
	}

	private static byte getReading(int value) {
		if (value > 1000)
			return SUMO_NOTHING;
		if (value > 600)
			return SUMO_LEFT;
		if (value > 400)
			return SUMO_RIGHT;
		return SUMO_CENTER;
	}

	private static void startMovePot() throws InterruptedException {

		boolean b = true;
		while (b) {
			leftMotor.forward();
			rightMotor.forward();

			sonic.getDistanceMode().fetchSample(sonarReadings, 0);
			float d = (int) (sonarReadings[0] * 1000) / 10f;
			LCD.drawString("front D " + d + "  ", 0, 0);

			if (d < 10) {
				leftMotor.stop(true);
				rightMotor.stop(true);
				b = false;
				LCD.drawString("BREAK  ", 0, 2);
				return;
			}

			iRSensor.fetchSample(iRreadings, 0);
			float di = iRreadings[0] * 10;

			if (di > sideSensorDistance + 4) {
				leftMotor.setSpeed(300);
				rightMotor.setSpeed(175);
				Thread.sleep(400);
				leftMotor.setSpeed(300);
				rightMotor.setSpeed(300);
			}
			if (di < sideSensorDistance) {
				rightMotor.setSpeed(300);
				leftMotor.setSpeed(175);
				Thread.sleep(400);
				leftMotor.setSpeed(300);
				rightMotor.setSpeed(300);
			}

		}

	}


	private static void findBlackSheet() throws InterruptedException {
		colorSampler.fetchSample(colourReadings, 0);
		int r = (int) (colourReadings[0] * 1000);
		int g = (int) (colourReadings[1] * 1000);
		int b = (int) (colourReadings[2] * 1000);

		boolean flip  = false;


		if((r < 10 && g < 10 && b < 10) && flip == false){
			flip = true;
			//loopTimes--;
			Button.LEDPattern(3);
		}
		 else if(r < 10 && g < 10 && b < 10){
			 flip = true;
		 }
		 else{
			 flip = false;
		 }

	}

}