import java.io.*;
import java.net.ServerSocket;
import java.net.Socket;

import lejos.hardware.lcd.LCD;


// This is the Robot-to-RTMS communications class
public class RBtoPCcom {
	public static final int 	SERVER_PORT = 6000; 	// arbitrary, but above 1023

	public static final byte	CONNECT_ME = 0;			// Tell the RTMS that this robot is up-and-running ...
	public static final byte	DEFINE_NEW_TRACE = 1;	// Ask the RTMS to make a new trace ...
	public static final byte	REQUEST_START = 2;		// Request the start signal from the RTMS
	public static final byte	GET_POSE = 3;			// Request the current pose from the RTMS
	public static final byte	SEND_POSE = 4;			// Send a pose to the RTMS, to be attached to a trace
	public static final byte	REQUEST_RUN_STATUS = 5;	// Request the run status of the trace
	public static final byte	RESET_TRACE = 6;		// Reset a trace's data
	public static final byte	DEFINE_OBSTACLE_SET = 7;// Ask the RTMS to make a new obstacle set
	public static final byte	SEND_OBSTACLE = 8;		// Send an obstacle estimate to the RTMS, to be attached to an obstacle set
	public static final byte	SEND_READING = 9;		// Send a distance reading to the RTMS, to be mapped
	public static final byte	RESET_MAP = 10;			// Clear the current map
	public static final byte	DEBUG_PRINT = 11;		// Send a string to the RTMS for debugging
	public static final byte	MOVED_FORWARD = 12;		// Tell the RTMS that the robot moved forward a certain amount (for localization)
	public static final byte	TURNED = 13;			// Tell the RTMS that the robot turned a certain amount (for localization)
	public static final byte	SEE_AHEAD = 14;			// Tell the RTMS that the robot sees an object at a certain distance ahead (for localization)
	public static final byte	SHUT_DOWN = 15;			// Tell the RTMS that the robot is all done now and shutting down

	private ServerSocket		serverSocket;
	private Socket				socket;
	private DataInputStream 	dis;
	private DataOutputStream 	dos;
	private boolean 			connected;				// Indicates whether or not robot has connected to the RTMS

	// Constructor that attempts to connect to the RTMS
	public RBtoPCcom() {
		connected = false;
		if (resetServerSocket()) {
			try {
				LCD.drawString("Waiting for RTMS  ", 0, 7);
				socket = serverSocket.accept();		// waits here

				// At this point, a client connection has been made
				dis = new DataInputStream(socket.getInputStream());
				dos = new DataOutputStream(socket.getOutputStream());

				connected = true;
				LCD.drawString("RTMS Connected    ", 0, 7);
			}
			catch (IOException e) {
				// Something did not work ... so close it down
				try {
					if (dos != null)
						dos.close();
					if (dis != null)
						dis.close();
					if (socket != null)
						socket.close();
				}
				catch(IOException ex) {}
				LCD.drawString("RTMS Connect ERROR", 0, 7);
			}
		}
	}

	// Start up a new server socket to accept a connection from the PC
	private boolean resetServerSocket() {
		// Make sure the old socket is closed
		closeServerSocket();

		serverSocket = null;
		try {
			serverSocket = new ServerSocket(SERVER_PORT);
			LCD.drawString("SERVER Online     ", 0, 7);
			return true;
		}
		catch (IOException e) {
			LCD.drawString("SERVER ERROR      ", 0, 7);
		}
		return false;
	}

	// Close the connection to the PC as well as the server socket
	private void closeServerSocket() {
		try {
			if (dis != null)
				dis.close();
			if (dis != null)
				dis.close();
			if (socket != null)
				socket.close();
		}
		catch (IOException e) {
			LCD.drawString("ERROR CLOSING     ", 0, 7);
		}
		if (serverSocket != null) {
			try {
				serverSocket.close();
			}
			catch (IOException e) {
				LCD.drawString("ERROR CLOSING     ", 0, 7);
			}
		}
	}

	// Return true if the EV3 is connected to the RTMS at the moment and false otherwise.
	public boolean isConnected() {
		return connected;
	}

	// Close the connection to the RTMS
	public void closeConnection() {
		LCD.drawString("Shutting down..." ,0, 7);
		try {
			dos.writeByte(SHUT_DOWN);
			dos.flush();
			LCD.drawString("Sent SHUT DOWN    " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Shut Down  ", 0, 7);
		}

		// Shut it all down now
		closeServerSocket();
	}

	// Send data to the RTMS to define a new trace with the given name and color.
	// Wait for the trace ID to be returned
	public int defineTrace(String name, int r, int g, int b) {
		LCD.drawString("Define Trace      ", 0, 7);
		try {
			dos.writeByte(DEFINE_NEW_TRACE);
		    dos.writeInt(r);	// Trace color (Red amount)
		    dos.writeInt(g);	// Trace color (Green amount)
		    dos.writeInt(b);	// Trace color (Blue amount)
		    dos.writeUTF(name);
		    dos.flush();

		    // Now get and return the trace ID from the RTMS
		    int id = dis.readInt();
		    LCD.drawString("Got Trace ID      ", 0, 7);
		    return id;
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Def Trace  ", 0, 7);
			return -1;
		}
	}

	// Send data to the RTMS to define a new obstacle set with the given name and color.
	// Wait for the index to be returned
	public int defineObstacleSet(String name, int r, int g, int b) {
		LCD.drawString("Define Obs Set    ", 0, 7);
		try {
			dos.writeByte(DEFINE_OBSTACLE_SET);
		    dos.writeInt(r);	// Trace color (Red amount)
		    dos.writeInt(g);	// Trace color (Green amount)
		    dos.writeInt(b);	// Trace color (Blue amount)
		    dos.writeUTF(name);
		    dos.flush();

		    // Now get and return the index from the RTMS
		    int id = dis.readInt();
		    LCD.drawString("Got Set Index     ", 0, 7);
		    return id;
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Def Obs Set", 0, 7);
			return -1;
		}
	}

	// Send a request to the RTMS to start and then wait until an ACK byte is sent
	// back (which occurs when the user presses the start button).
	public void waitForStart() {
		LCD.drawString("Request Start     " ,0, 7);
		try {
			dos.writeByte(REQUEST_START);
		    dos.flush();
		    dis.readByte();		// Now wait for ACK
		    LCD.drawString("Got Start ACK     ", 0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Req Start  ", 0, 7);
		}
	}

	// Send a request to the RTMS for the robot's starting position and then
	// wait until it is sent back.   Return the position as an array of 3 floats
	public Pose requestPosition() {
		LCD.drawString("Request Position  " ,0, 7);
		Pose p = new Pose();
		try {
			dos.writeByte(GET_POSE);
		    dos.flush();
		    p.x = dis.readFloat();
		    p.y = dis.readFloat();
		    p.angle = dis.readFloat();
		    LCD.drawString("Got Position      " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Req Pose   ", 0, 7);
			p.x = p.y = p.angle = -1;
		}
		return p;
	}

	// Send an (x,y,angle) position to the RTMS to be appended to the trace with the given ID.
	public void sendPosition(int trace, Pose p) {
		LCD.drawString("Sending Position  " ,0, 7);
		try {
			dos.writeByte(SEND_POSE);
			dos.writeInt(trace);
			dos.writeFloat(p.x);
			dos.writeFloat(p.y);
			dos.writeFloat(p.angle);
			dos.flush();
			LCD.drawString("Sent Position     " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Send Pose  ", 0, 7);
		}
	}

	// Send an (x,y) obstacle position to the RTMS to be appended to the obstacle set with the given index.
	public void sendObstacle(int index, float x, float y) {
		LCD.drawString("Sending Obs Pos.  " ,0, 7);
		try {
			dos.writeByte(SEND_OBSTACLE);
			dos.writeInt(index);
			dos.writeFloat(x);
			dos.writeFloat(y);
			dos.flush();
			LCD.drawString("Sent Obs Pos.     " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Send Pose  ", 0, 7);
		}
	}

	// Send a distance reading to the RTMS to be mapped.
	public void sendReading(int index, float x, float y, float angle, float d, float aOff, float dError, float beamAngle) {
		LCD.drawString("Sending Reading   " ,0, 7);
		try {
			dos.writeByte(SEND_READING);
			dos.writeInt(index);
			dos.writeFloat(x);
			dos.writeFloat(y);
			dos.writeFloat(angle);
			dos.writeFloat(d);
			dos.writeFloat(aOff);
			dos.writeFloat(dError);
			dos.writeFloat(beamAngle);
			dos.flush();
			LCD.drawString("Sent Reading      " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Send Readng", 0, 7);
		}
	}


	// Send an update to the RTMS to tell it that the robot moved forward a certain distance (in CM) - for localization
	public void sendMovedForward(float d) {
		LCD.drawString("Sending MV_FWD  " ,0, 7);
		try {
			dos.writeByte(MOVED_FORWARD);
			dos.writeFloat(d);
			dos.flush();
			LCD.drawString("Sent MV_FWD       " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Send MV_FWD", 0, 7);
		}
	}

	// Send an update to the RTMS to tell it that the robot turned a certain amount (in DEG) - for localization
	public void sendTurned(float d) {
		LCD.drawString("Sending TURNED  " ,0, 7);
		try {
			dos.writeByte(TURNED);
			dos.writeFloat(d);
			dos.flush();
			LCD.drawString("Sent TURNED       " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Send TURNED", 0, 7);
		}
	}

	// Send an update to the RTMS to tell it that the robot detects an object ahead at the given distance (in CM) - for localization
	public void sendDistanceAhead(float d) {
		LCD.drawString("Sending SEE     " ,0, 7);
		try {
			dos.writeByte(SEE_AHEAD);
			dos.writeFloat(d);
			dos.flush();
			LCD.drawString("Sent SEE          " ,0, 7);
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Send SEE   ", 0, 7);
		}
	}


	// Send data to the RTMS to clear a trace and start over with the given pose
	public void resetTrace(int id, Pose p) {
		LCD.drawString("Clear Trace      ", 0, 7);
		try {
			dos.writeByte(RESET_TRACE);
		    dos.writeInt(id);	// Trace id
			dos.writeFloat(p.x);
			dos.writeFloat(p.y);
			dos.writeFloat(p.angle);
		    dos.flush();
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Def Trace  ", 0, 7);
		}
	}


	// Send a float to the RTMS for debug purposes
	public void debugPrintln(String s) {
		LCD.drawString("Debug print       ", 0, 7);
		try {
			dos.writeByte(DEBUG_PRINT);
		    dos.writeUTF(s);
		    dos.flush();
		}
		catch(Exception ex) {
			LCD.drawString("ERROR: Debug Print", 0, 7);
		}
	}
}