import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.robotics.SampleProvider;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
/*
 * Main class for a behavior-based robot. The robot is assumed to have two motors,
 * one forward-facing distance sensor, and one downward-facing light/color sensor.
 * 
 * It defines all fields of our robot as well as some basic capabilities
 * such as 'learnBW' to learn black and white. Includes the main method to define
 * the various behaviors, add them to a list, and start the arbitrator.
 */
public class MyMain 
{
	// Defining the various speeds
	public final static int ATTACK_SPEED = 600;
	public final static int NORMAL_SPEED = 300;
	public static final int TURN_SPEED = 150;
		
	// Left and right motors - note their attached ports
	public static EV3LargeRegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.A); 
	public static EV3LargeRegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.D); 

	// Distance sensor - note its attached port
	public static EV3UltrasonicSensor distSensor = new EV3UltrasonicSensor(SensorPort.S4);
	public static SampleProvider distProvider = distSensor.getDistanceMode();
	public static float[] distData = new float[distProvider.sampleSize()];

	// Light/color sensor - note its attached port
	public static EV3ColorSensor lightSensor = new EV3ColorSensor(SensorPort.S1);
	public static SampleProvider lightProvider = lightSensor.getRedMode();
	public static float[] lightData = new float[lightProvider.sampleSize()];
	
	// Defines boundary between white and black
	public static float zero = 0.0f;
	
	// Returns distance to distance sensor
	public static double getDistance()
	{
		distProvider.fetchSample(distData, 0);
		return distData[0];
	}
	
	// Returns current light intensity
	public static float getLight()
	{
		lightProvider.fetchSample(lightData, 0);
		return lightData[0];
	}
	
	// Displays message and waits for a button press
	public static void waitForSignal(String msg)
	{
		Sound.beep();
		System.out.println(msg);
		Button.ENTER.waitForPressAndRelease();
	}
	
	// Learns intensities of 'black' and 'white'
	public static void learnBW()
	{
		waitForSignal("White + ENTER");
		float white = getLight();
		
		waitForSignal("Black + ENTER");
		float black = getLight();
		
		zero = (white + black) / 2;
		System.out.println("Black: " + black);
		System.out.println("White: " + white);
		System.out.println("Zero:  " + zero);
		waitForSignal("ENTER to go");
	}
	
	// Main method. Creates the behaviors, adds them to a list, and
	// starts the arbitrator thread.
	public static void main(String[] args)
	{
		learnBW();

		Drive drive = new Drive();
		Bounce bounce = new Bounce();
		
		Behavior[] behaviors = { drive, bounce };
		Arbitrator arbitrator = new Arbitrator(behaviors);
		arbitrator.go();
	}
}