package LezCW;
import lejos.hardware.motor.Motor;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

public class LEZCOMEHOME {

    // Constructors for sensors.
    private static EV3UltrasonicSensor us1 = new
            EV3UltrasonicSensor(SensorPort.S1);


    private static EV3TouchSensor touch2 = new
            EV3TouchSensor(SensorPort.S2);

    private static EV3TouchSensor touch3 = new
            EV3TouchSensor(SensorPort.S3);

    /* private static EV3GyroSensor gyroSensor = new
             EV3GyroSensor(SensorPort.S2);
    private static EV3ColorSensor color1 = new
            EV3ColorSensor(SensorPort.S4);
 	*/

    /*
     * Basic testing.
     */
    public static void main(String[] args) throws InterruptedException {
        Motor.A.setSpeed(300);
        Motor.B.setSpeed(300);
        us1.enable();
        while (true){
//        	while(getSonicDist() > 0.35) {
        	while(isTouched() == 0) {
         		Motor.A.forward();
         		Motor.B.forward();
         	}
        	Motor.A.stop();
        	Motor.B.stop();
        	turn(getDirection());
        }
    }

//        for (int i = 0; i < 100; i++) {
//
//        	Delay.msDelay(1000);
////            float sonicDist = getSonicDist();
////            if (sonicDist > 0.2) {
////                Motor.A.forward();
////                Motor.B.forward();
////                Delay.msDelay(300);
////                Motor.A.stop();
////                Motor.B.stop();
////            } else {
////            	Motor.A.backward();
////                Motor.B.backward();
////                Delay.msDelay(500);
////                Motor.A.forward();
////                Motor.B.backward();
////                Delay.msDelay(350);
////                Motor.A.stop();
////                Motor.B.stop();
////            }
//        }
//        us1.disable();

    /*
     * Returns the distance currently read by the ultrasonic sensor and prints value to
     * EV3 screen.
     */
    public static float getSonicDist(){
        // Measures distance to object in front of sensor and stores in sp.
        final SampleProvider sp = us1.getDistanceMode();
        float distanceValue = 0;
        // Creates a float array with size equal to the number of elements in the sensor sample.
        float [] sample = new float[sp.sampleSize()];
        // Fetches the sample from sp with offset 0 and stores in sample array.
        sp.fetchSample(sample, 0);
        // Gets first index of sample array and stores in distanceValue.
        distanceValue = sample[0];
        System.out.println("Ultrasonic distance: " + distanceValue);
        return distanceValue;
    }

 	/* Calculates the diretion to travel
 	 *
 	 */
	public static char getDirection(){
		char direction = 's';
		int[] rotationAngle = {45, -90};// rotate right then left
		float[] directionDistance = {0,0};
		for(int i = 0; i <2; i++){
			Motor.C.rotate(rotationAngle[i]);
			directionDistance[i] = getSonicDist();
		}
		if(directionDistance[0]>directionDistance[1]){
			direction = 'r'; // if left wall is closer turn right
		}else{
			direction = 'l';
		}
		Motor.C.rotate(45);
		return direction;
	}

	public static void turn(char dir){
		switch(dir){
			case 'r':
				Motor.A.backward();
				Motor.B.backward();
				Delay.msDelay(100);
				Motor.A.forward();
				Motor.B.backward();
	        	Delay.msDelay(250); //determines turn size
	        	Motor.A.stop();
	        	Motor.B.stop();
				break;
			case'l':
				Motor.A.backward();
				Motor.B.backward();
				Delay.msDelay(100);
				Motor.B.forward();
				Motor.A.backward();
	        	Delay.msDelay(250); //determines turn size
	        	Motor.A.stop();
	        	Motor.B.stop();
				break;
			default:
				break;
		}
	}

	  /*
	  * Returns a 1 if touch sensor is pressed and a 0 if not. GB - THIS IS A GUESS, MAY BE OPPOSITE.
	  */
	 public static int isTouched() {
	     final SampleProvider sp2 = touch2.getTouchMode();
	     final SampleProvider sp3 = touch3.getTouchMode();
	     int touchValue2 = 0;
	     int touchValue3 = 0;
	     float [] sample2 = new float[sp2.sampleSize()];
	     float [] sample3 = new float[sp3.sampleSize()];
	     sp2.fetchSample(sample2, 0);
	     sp3.fetchSample(sample3, 0);
	     touchValue2 = (int)sample2[0];
	     touchValue3 = (int)sample3[0];
	     System.out.println("Touch 2: " + touchValue2);
	     System.out.println("Touch 3: " + touchValue3);
	     if (touchValue2 == 1 || touchValue3 == 1) {
	    	 return 1;
	     } else {
	    	 return 0;
	     }
	 }
}
    /*
     * Returns the angle currently read by the gyro sensor and prints value to
     * EV3 screen.
     */
//    public static int getGyroAngle() {
//        // Measures the orientation of the sensor in respect to its start orientation.
//        final SampleProvider sp = gyroSensor.getAngleMode();
//        int angleValue = 0;
//        float [] sample = new float[sp.sampleSize()];
//        sp.fetchSample(sample, 0);
//        angleValue = (int)sample[0];
//        System.out.println("Gyro angle: " + angleValue);
//        return angleValue;
//    }
//
//    /*
//     * Returns a 1 if touch sensor is pressed and a 0 if not. GB - THIS IS A GUESS, MAY BE OPPOSITE.
//     */
//    public static int isTouched() {
//        final SampleProvider sp = touch1.getTouchMode();
//        int touchValue = 0;
//        float [] sample = new float[sp.sampleSize()];
//        sp.fetchSample(sample, 0);
//        touchValue = (int)sample[0];
//        System.out.println("Touch: " + touchValue);
//        return touchValue;
//    }
//
//    /*
//     * Returns RBG detected of colour sensor. GB - NOT SURE ABOUT THE VALUES IN SAMPLE[]
//     */
//    public static float[] getRGB() {
//        SampleProvider sp = color1.getRGBMode();
//        float[] sample = new float[sp.sampleSize()];
//        sp.fetchSample(sample, 0);
//        System.out.println("R: " + (int)sample[0] + " G: " + (int)sample[1] + " B: " + (int)sample[2]);
//        return sample;
//    }
//
//}