// SimpleSensor.java
// 
// A simple demo that illustrates how to use the Sensor Framework and also
// display graphics and small text on the display under LeJOS on the Lego EV3.
//
// Terry Payne
// 26th September 2017
// 
// This demo defines two methods.  The introMessage method illustrates
// how to make use of the GraphicsLCD class to generate text that is
// small, and also illustrates how to draw on the screen.  The class also
// includes a number of other graphics primitives.
// 
// The main method uses the sample provider framework to get samples from
// the touch sensor, and then interrogate them to start and stop the motors.
// Note that in this scenario, we just use a simple motor class. 
// 
// Check out https://sourceforge.net/p/lejos/wiki/Sensor%20Framework/ for
// more information about using the Sensor Framework.


import lejos.hardware.Button;
import lejos.hardware.lcd.Font;
import lejos.hardware.lcd.GraphicsLCD;
import lejos.hardware.ev3.LocalEV3;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.robotics.SampleProvider;
import lejos.hardware.motor.Motor;

public class SimpleSensor {

	public static void introMessage() {
		
		GraphicsLCD g = LocalEV3.get().getGraphicsLCD();
		g.drawString("Simple Sensor", 5, 0, 0);
		g.setFont(Font.getSmallFont());
		 
		g.drawString("This demonstrates how to ", 2, 20, 0);
		g.drawString("draw and change fonts on the", 2, 30, 0);
		g.drawString("Display, and use the Sensor ", 2, 40, 0);
		g.drawString("famework. ", 2, 50, 0);
		g.drawString("To use the motors:", 2, 60, 0);
		g.drawString("Left Bumper - start", 30, 70, 0);
		g.drawString("Right Bumper - stop", 30, 80, 0); 
		  
		// Quit GUI button:
		g.setFont(Font.getSmallFont()); // can also get specific size using Font.getFont()
		int y_quit = 100;
		int width_quit = 45;
		int height_quit = width_quit/2;
		int arc_diam = 6;
		
		g.drawString("QUIT", 9, y_quit+7, 0);
		g.drawLine(0, y_quit,  45, y_quit); // top line
		g.drawLine(0, y_quit,  0, y_quit+height_quit-arc_diam/2); // left line
		g.drawLine(width_quit, y_quit,  width_quit, y_quit+height_quit/2); // right line
		g.drawLine(0+arc_diam/2, y_quit+height_quit,  width_quit-10, y_quit+height_quit); // bottom line
		g.drawLine(width_quit-10, y_quit+height_quit, width_quit, y_quit+height_quit/2); // diagonal
		g.drawArc(0, y_quit+height_quit-arc_diam, arc_diam, arc_diam, 180, 90);
				
	}

	public static void main(String[] args) {
		introMessage();
		
		// 1) Get a port instance
		Port leftPort = LocalEV3.get().getPort("S2");
		Port rightPort = LocalEV3.get().getPort("S1");

		// 2) Get an instance of the sensor on the port
		EV3TouchSensor leftBump = new EV3TouchSensor(leftPort);
		EV3TouchSensor rightBump = new EV3TouchSensor(rightPort);
		
		// 3) Get an instance of the sample provider (in this case the value is 0 or 1)
		SampleProvider leftTouch= leftBump.getMode("Touch");
		SampleProvider rightTouch= rightBump.getMode("Touch");

		// 4) Initialise an array of floats for fetching samples
		// 		Ask the SampleProvider how long the array should be
		float[] leftSample = new float[leftTouch.sampleSize()];
		float[] rightSample = new float[rightTouch.sampleSize()];
		
        while (!Button.ESCAPE.isDown()) {

        	// 5) Get a sample
        	leftTouch.fetchSample(leftSample, 0);
        	if (leftSample[0] == 1.0) {
        		// sensor was pressed
        		Motor.B.forward();
        		Motor.C.forward();
        	}
        	rightTouch.fetchSample(rightSample, 0);
        	if (rightSample[0] == 1.0) {
        		// sensor was pressed
        		Motor.B.stop();
        		Motor.C.stop();
        	}
        }
        
        // 6) Remember to close the sensor when done
        leftBump.close();
        rightBump.close();
	}
}