import lejos.nxt.*;
import java.io.*;
import lejos.nxt.comm.*;
import lejos.util.Delay;
import lejos.robotics.navigation.DifferentialPilot;

public class RemoteControl {
    public static DataOutputStream dataOut;
    public static DataInputStream dataIn;
    public static NXTConnection Link;
    public static DifferentialPilot pilot;
    public static LightSensor sensor;


    public static int speed = 200, turnSpeed = 125, speedBuffer, speedControl;
    public static int commandValue,transmitReceived, lastCommand = 0;

    public static void main(String [] args) throws Exception {
        connect();
        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.A, Motor.C, false);
        sensor = new LightSensor(SensorPort.S4) ;

        while(true){
            if (dataIn.available() > 0){
                byte x = dataIn.readByte();
                byte y = dataIn.readByte();
                if(checkCommand((int)x, (int)y)) {
                    disconnect();
                    break;
                }
            }
        }
    }
 
    public static boolean checkCommand(int cmd, int data) {
        System.out.println(data);
        try {
            switch (data) {
                case 0x1:
                    pilot.travel(data);
                    dataOut.writeChars("OK");
                    break;
                case 0x2:
                    pilot.rotate(data/255.0*360);
                    dataOut.writeChars("OK");
                    break;
                case 0x3:
                    int v = sensor.readValue();
                    dataOut.writeChars(String.valueOf(v));
                    break;
            }
            dataOut.flush();
        } catch (IOException e) {
        }
        return false;
    }

    public static void connect() {  
        System.out.println("Listening..");
        Link = Bluetooth.waitForConnection(0, NXTConnection.RAW);
        dataOut = Link.openDataOutputStream();
        dataIn = Link.openDataInputStream();

    }

    public static void disconnect() throws java.io.IOException {  
        System.out.println("Closing...");
        dataOut.close();
        dataIn.close();
        USB.usbReset();
        System.exit(0);
    }
}