import java.io.*;
import java.net.*;
import java.util.*;
import josx.rcxcomm.*;

/**
 * A remote control client for use with the RCX Remote Control Server.
 * It works with the Java Colour Tracker so that the robot can follow an object of
 * interest. Remember to change the port that the IR tower is actually connected to below.
 *
 * @author David (www.uk-dave.com)
 * @version 1.0, 23/03/2004
 */
public class VisionPCClient
{
	////////////////////////////////////////////////////
	// CONSTANTS AND CLASS VARIABLES
	////////////////////////////////////////////////////

	public static final byte STOP=0x00, SET_POWER=0x05, PLAY_SYSTEM_SOUND=0x10;
	public static final byte FORWARD=0x01, FORWARD_THEN_STOP=0x11;
	public static final byte BACKWARD=0x02, BACKWARD_THEN_STOP=0x12;
	public static final byte LEFT=0x03, LEFT_THEN_STOP=0x13;
	public static final byte RIGHT=0x04, RIGHT_THEN_STOP=0x14;

	public static final String IR_TOWER_PORT = "COM2";
	public static final String VISION_SERVER_ADDRESS = "localhost";
	public static final int VISION_SERVER_PORT = 5500;
	public static final int IMAGE_WIDTH=320, IMAGE_HEIGHT=240;
	public static final int LOST=0;

	private static RCXBean rcx;
	private static byte currentCommand=STOP;

	private static Socket visionSocket;
	private static BufferedReader in;
	private static PrintWriter out;
	private static int objectState=LOST;






	////////////////////////////////////////////////////
	// MAIN
	////////////////////////////////////////////////////

	/**
	 *
	 */
	public static void main(String[] args)
	{
		// Connect to the IR tower
		rcx = new RCXBean();
		try { rcx.setComPort(IR_TOWER_PORT); }
		catch (IOException ioe) { System.out.println("Error communicationg with IR tower!"); System.exit(1); }

		// Connect to the Colour Tracker server
		try
		{
			visionSocket = new Socket(VISION_SERVER_ADDRESS, VISION_SERVER_PORT);
			in = new BufferedReader(new InputStreamReader(visionSocket.getInputStream()));
			out = new PrintWriter(visionSocket.getOutputStream(), true);
		}
		catch (IOException ioe) { System.out.println("Error connecting to Colour Tracker server"); System.exit(1); }

		int X1,Y1,X2,Y2, Xcenter, Ycenter;
		try
		{
			sendCommand(STOP);
			sendCommand(SET_POWER);
			sendValue((byte)0x03);
			sendCommand(PLAY_SYSTEM_SOUND);
			sendValue((byte)0x01);
			String line = in.readLine();
			for (;;)
			{
				out.println("Get coords");
				line = in.readLine();
				if (line.startsWith("Object lost!"))
				{
					if (objectState!=LOST)
					{
						objectState=LOST;
						System.out.println("Lost object!");
						sendCommand(PLAY_SYSTEM_SOUND); sendValue((byte)0x02);
					}
					sendCommand(RIGHT_THEN_STOP); sendValue((byte)0x05);
				}
				else if (line.startsWith("Found object!"))
				{
					StringTokenizer split = new StringTokenizer(line.substring(line.indexOf(':')+2), ",");
					X1 = Integer.parseInt(split.nextToken());
					Y1 = Integer.parseInt(split.nextToken());
					X2 = Integer.parseInt(split.nextToken());
					Y2 = Integer.parseInt(split.nextToken());

					if ((X2-X1)>5 && (Y2-Y1)>5)
					{
						if (objectState==LOST)
						{
							sendCommand(PLAY_SYSTEM_SOUND); sendValue((byte)0x03);
						}
						Xcenter = ((X2-X1)/2)+X1;
						if (Xcenter>((IMAGE_WIDTH/2)+25))
						{
							if (objectState!=LEFT)
							{
								objectState=LEFT;
								sendCommand(PLAY_SYSTEM_SOUND); sendValue((byte)0x00);
							}
							System.out.println("Object is on the left");
							sendCommand(LEFT_THEN_STOP); sendValue((byte)0x01);
						}
						else if (Xcenter<((IMAGE_WIDTH/2)-25))
						{
							if (objectState!=RIGHT)
							{
								objectState=RIGHT;
								sendCommand(PLAY_SYSTEM_SOUND); sendValue((byte)0x00);
							}
							System.out.println("Object is on the right");
							sendCommand(RIGHT_THEN_STOP); sendValue((byte)0x01);
						}
						else
						{
							if (objectState!=FORWARD)
							{
								objectState=FORWARD;
								sendCommand(PLAY_SYSTEM_SOUND); sendValue((byte)0x00);
							}
							System.out.println("Object is ahead");
							sendCommand(FORWARD_THEN_STOP); sendValue((byte)0x10);
						}
					}
					else
					{
						if (objectState!=LOST)
						{
							objectState=LOST;
							System.out.println("Lost object!");
							sendCommand(PLAY_SYSTEM_SOUND); sendValue((byte)0x02);
						}
						sendCommand(RIGHT_THEN_STOP); sendValue((byte)0x05);
					}
				}
				try { Thread.sleep(2000); }
				catch (InterruptedException e) {}
			}
		}
		catch (Exception e)
		{
			e.printStackTrace();
			rcx.close();
			try { visionSocket.close(); }
			catch (IOException ioe) {}
			System.exit(1);
		}
	}






	////////////////////////////////////////////////////
	// METHODS
	////////////////////////////////////////////////////


	/**
	 *
	 * @param command
	 */
	public static void sendCommand(byte command)
	{
		try
		{
			//if (command!=currentCommand)
			//{
				rcx.send(command);
				currentCommand = command;
			//}
		}
		catch (IOException ioe) { System.out.println("Error communicationg with IR tower!"); System.exit(1); }
	}


	/**
	 *
	 * @param value
	 */
	public static void sendValue(byte value)
	{
		try { rcx.send(value); }
		catch (IOException ioe) { System.out.println("Error communicationg with IR tower!"); System.exit(1); }
	}

}