Colony

//

//  Colonet.java

//

import javax.swing.*;

import javax.swing.event.*;

import javax.imageio.*;

import java.awt.*;

import java.awt.image.*;

import java.awt.event.*;

import java.net.*;

import java.io.*;

import java.util.*;

/**

*        The Colonet Graphical User Interface Applet for use locally and over an internet connection.

*        @author Gregory Tress

*        

*        To generate javadoc on this file or other java files, use javadoc *.java -d doc, where doc

*        is the name of the folder into which the files should be written.

*/

public class Colonet extends JApplet implements ActionListener, MouseInputListener, KeyListener, Runnable {

        // Used for images

        final int CANVAS_SIZE = 500;  //the applet may be slow if the canvas gets too large

        final int BUFFER = 50;

        final int RADIUS = 30;

        //Used for the robot controller

        final int VECTOR_CONTROLLER_HEIGHT = 220;

        final int VECTOR_CONTROLLER_WIDTH = 350;

        // Connection

        JTextField txtHost;                                

        JTextField txtPort;                                

        JButton btnConnect;        

        JButton btnGetXBeeIDs;

        JLabel lblConnectionStatus;

        JTextArea txtMatrix;

        JTextArea txtInfo; 

        JPanel panelConnect;

        JPanel panelServerInterface;

        Socket socket;                                        

        OutputStreamWriter out;

        DataUpdater dataUpdater;  

        // South

        JPanel panelSouth;

        JTextArea log;

        JScrollPane spLog;

        // Control

        JPanel panelControl;

        JTabbedPane tabPaneControl;

        JPanel panelRobotControl;

        JPanel panelRobotDirection;

        JPanel panelRobotDirectionButtons;

        JPanel panelRobotCommands;

        JButton btnF, btnB, btnL, btnR, btnActivate;

        JComboBox cmbRobotNum;

        JLabel lblBattery;

        JLabel lblSelected;

        BatteryIcon batteryIcon;

        JPanel panelBattery;

        VectorController vectorController;

        BufferedImage imageVectorControl;

        JButton btnAssignID;

        boolean setWaypoint;

        int setWaypointID;

        JButton btnCommand_MoveTo;

        JButton btnCommand_MoveAll;

        JButton btnCommand_StopTask;

        JButton btnCommand_ResumeTask;

        JButton btnCommand_ChargeNow;

        JButton btnCommand_StopCharging;

        // Task Manager

        JPanel panelTaskManager;

        JScrollPane spTaskManager;

        JPanel panelTaskManagerControls;

        JPanel panelTaskManagerControlsPriority;

        DefaultListModel taskListModel;

        JList taskList;

        JButton btnAddTask;

        JButton btnRemoveTask;

        JButton btnMoveTaskUp;

        JButton btnMoveTaskDown;

        JButton btnUpdateTasks;

        TaskAddWindow taskAddWindow;

        //Webcam

        WebcamPanel panelWebcam;

        GraphicsPanel panelGraph;

        GraphicsConfiguration gc;

        volatile BufferedImage image;

        volatile Graphics2D canvas;

        int cx, cy;

        JTabbedPane tabPaneMain;

        Font botFont;

        volatile int numBots;

        volatile int selectedBot;  //the user has selected this bot graphically

        volatile java.util.List <RobotIcon> robotIcons;  //contains boundary shapes around bots for click detection

        volatile int[] xbeeID;

        Thread paintThread;

        SelectionIndicator indicator;

        WebcamLoader webcamLoader;

        ColonetServerInterface csi;

        public void init () {

                // Set the default look and feel - choose one

        //String laf = UIManager.getSystemLookAndFeelClassName();

                String laf = UIManager.getCrossPlatformLookAndFeelClassName();

                //String laf = "com.sun.java.swing.plaf.motif.MotifLookAndFeel";

        try {

            UIManager.setLookAndFeel(laf);

        } catch (UnsupportedLookAndFeelException exc) {

            System.err.println ("Warning: UnsupportedLookAndFeel: " + laf);

        } catch (Exception exc) {

            System.err.println ("Error loading " + laf + ": " + exc);

        }

                // We should invoke and wait to avoid browser display difficulties

                Runnable r = new Runnable() {

                        public void run() {

                                createAndShowGUI();

                        }

                };

                try {

                        SwingUtilities.invokeAndWait(r);

                } catch (InterruptedException e) {

                        //Not really sure why we would be in this situation

                        System.out.println("InterruptedException in init: " + e);

                } catch (java.lang.reflect.InvocationTargetException e) {

                        //This could happen for various reasons if there is a problem in createAndShowGUI

                        e.printStackTrace();

                }

        }

        public void destroy () {

                try { paintThread.interrupt(); } catch (Exception e) { }

                try { indicator.interrupt(); } catch (Exception e) { }

        }

        private synchronized void createAndShowGUI () {

                // init graphical elements

                // Get the graphics configuration of the screen to create a buffer

                gc = GraphicsEnvironment.getLocalGraphicsEnvironment()

                        .getDefaultScreenDevice().getDefaultConfiguration();

                image = gc.createCompatibleImage(CANVAS_SIZE,CANVAS_SIZE);

                canvas = image.createGraphics();

                canvas.setStroke(new BasicStroke(2));  //set pen width

                panelGraph = new GraphicsPanel(image, false);  //set automatic double-buffering to false. we are doing it manually.

                panelWebcam = new WebcamPanel();

                tabPaneMain = new JTabbedPane();

                tabPaneMain.add(panelWebcam, "Webcam");

                //tabPaneMain.add(panelGraph, "Graph");

                // Calculate center of canvas

                cx = image.getWidth() / 2;

                cy = image.getHeight() / 2;

                // Set up robots

                botFont = new Font("Arial", Font.PLAIN, 14);

                numBots = 0;

                selectedBot = -1;

                robotIcons = new ArrayList <RobotIcon> ();

                // Connection area

                txtMatrix = new JTextArea();

                txtMatrix.setBorder(BorderFactory.createTitledBorder("Input Matrix"));

                txtInfo = new JTextArea();

                txtInfo.setBorder(BorderFactory.createTitledBorder("Info"));

                txtInfo.setEditable(false);

                txtHost = new JTextField(this.getDocumentBase().getHost());

                txtHost.setBorder(BorderFactory.createTitledBorder("Host"));

                txtPort = new JTextField("10123");

                txtPort.setBorder(BorderFactory.createTitledBorder("Port"));

                btnConnect = new JButton("Connect");

                btnGetXBeeIDs = new JButton("Get XBee IDs");

                getRootPane().setDefaultButton(btnConnect);

                lblConnectionStatus = new JLabel("Status: Offline");

                panelConnect = new JPanel();

                panelConnect.setLayout(new GridLayout(6,1));

                panelConnect.add(lblConnectionStatus);

                panelConnect.add(txtHost);

                panelConnect.add(txtPort);

                panelConnect.add(btnConnect);

                //panelConnect.add(btnGetXBeeIDs);

                panelServerInterface = new JPanel();

                panelServerInterface.setLayout(new GridLayout(2,1));

                panelServerInterface.add(panelConnect);

                panelServerInterface.add(txtMatrix);

                // Robot direction panel

                panelRobotDirection = new JPanel();

                panelRobotDirectionButtons = new JPanel();

                btnF = new JButton("^");

                btnB = new JButton("v");

                btnL = new JButton("<");

                btnR = new JButton(">");

                btnActivate = new JButton("o");

                panelRobotDirectionButtons.setLayout(new GridLayout(1,5));

                panelRobotDirectionButtons.add(btnActivate);

                panelRobotDirectionButtons.add(btnF);

                panelRobotDirectionButtons.add(btnB);

                panelRobotDirectionButtons.add(btnL);

                panelRobotDirectionButtons.add(btnR);

                imageVectorControl = gc.createCompatibleImage(VECTOR_CONTROLLER_WIDTH, VECTOR_CONTROLLER_HEIGHT);

                vectorController = new VectorController(imageVectorControl);

                panelRobotDirection.setLayout(new BorderLayout());

                panelRobotDirection.add(vectorController, BorderLayout.CENTER);

                panelRobotDirection.add(panelRobotDirectionButtons, BorderLayout.SOUTH);

                // Robot Control and Commands

                panelRobotCommands = new JPanel();

                panelRobotCommands.setLayout(new GridLayout(5,2));

                cmbRobotNum = new JComboBox();

                // Battery subset

                batteryIcon = new BatteryIcon(25);

                lblBattery = new JLabel(batteryIcon);

                lblSelected = new JLabel("None");

                // Command subset

                setWaypoint = false;

                setWaypointID = -1;

                btnAssignID = new JButton("Assign ID");

                btnCommand_MoveTo = new JButton("Move to ...");

                btnCommand_MoveAll = new JButton("Move all ...");

                btnCommand_StopTask = new JButton("Stop Current Task");

                btnCommand_ResumeTask = new JButton("Resume Current Task");

                btnCommand_ChargeNow = new JButton("Recharge Now");

                btnCommand_StopCharging = new JButton("Stop Recharging");

                panelRobotCommands.add(new JLabel("Select Robot to Control: "));

                panelRobotCommands.add(cmbRobotNum);

                panelRobotCommands.add(new JLabel("Battery Level: "));

                panelRobotCommands.add(lblBattery);

                panelRobotCommands.add(new JLabel("Selected Icon: "));

                panelRobotCommands.add(lblSelected);

                panelRobotCommands.add(btnAssignID);

                panelRobotCommands.add(new JLabel(""));

                panelRobotCommands.add(btnCommand_MoveTo);

                panelRobotCommands.add(btnCommand_MoveAll);

                //panelRobotCommands.add(btnCommand_StopTask);

                //panelRobotCommands.add(btnCommand_ResumeTask);

                //panelRobotCommands.add(btnCommand_ChargeNow);

                //panelRobotCommands.add(btnCommand_StopCharging);

                panelRobotControl = new JPanel();

                panelRobotControl.setLayout(new GridLayout(2,1));

                panelRobotControl.add(panelRobotDirection);

                panelRobotControl.add(panelRobotCommands);

                // Task Manager

                panelTaskManager = new JPanel();

                panelTaskManager.setLayout(new BorderLayout());

                taskListModel = new DefaultListModel();

                taskList = new JList(taskListModel);

                taskList.setSelectionMode(ListSelectionModel.SINGLE_SELECTION);

                taskList.setSelectedIndex(0);

                spTaskManager = new JScrollPane(taskList);

                panelTaskManagerControls = new JPanel();

                panelTaskManagerControls.setLayout(new GridLayout(1,4));

                panelTaskManagerControlsPriority = new JPanel();

                panelTaskManagerControlsPriority.setLayout(new GridLayout(1,2));

                btnAddTask = new JButton("Add...");

                btnRemoveTask = new JButton("Remove");

                btnMoveTaskUp = new JButton("^");

                btnMoveTaskDown = new JButton("v");

                btnUpdateTasks = new JButton("Update");

                panelTaskManagerControlsPriority.add(btnMoveTaskUp);

                panelTaskManagerControlsPriority.add(btnMoveTaskDown);

                panelTaskManagerControls.add(btnAddTask);

                panelTaskManagerControls.add(btnRemoveTask);

                panelTaskManagerControls.add(btnUpdateTasks);

                panelTaskManagerControls.add(panelTaskManagerControlsPriority);

                panelTaskManager.add(spTaskManager, BorderLayout.CENTER);

                panelTaskManager.add(panelTaskManagerControls, BorderLayout.SOUTH);

                panelTaskManager.add(new JLabel("Current Task Queue"), BorderLayout.NORTH);

                taskAddWindow = new TaskAddWindow();

                // Message log

                log = new JTextArea();

                spLog = new JScrollPane(log,

                        ScrollPaneConstants.VERTICAL_SCROLLBAR_ALWAYS, 

                        ScrollPaneConstants.HORIZONTAL_SCROLLBAR_AS_NEEDED);

                spLog.setBorder(BorderFactory.createTitledBorder("Log"));

                spLog.setPreferredSize(new Dimension(0, 120));

                log.setEditable(false);

                // Main control mechanism

                panelControl = new JPanel();

                panelControl.setLayout(new GridLayout(1,1));

                tabPaneControl = new JTabbedPane(JTabbedPane.TOP);

                tabPaneControl.setPreferredSize(new Dimension(VECTOR_CONTROLLER_WIDTH, 0));

                tabPaneControl.addTab("Connection", panelServerInterface);

                tabPaneControl.addTab("Robots", panelRobotControl);

                tabPaneControl.addTab("Tasks", panelTaskManager);

                panelControl.add(tabPaneControl);

                // Set up elements in the south

                panelSouth = new JPanel();

                panelSouth.setLayout(new GridLayout(1,2));

                //panelSouth.add(spLog);

                // Put all elements in the ContentPane

                this.getContentPane().setLayout(new BorderLayout());

                this.getContentPane().add(tabPaneMain, BorderLayout.CENTER);

                this.getContentPane().add(panelSouth, BorderLayout.SOUTH);

                this.getContentPane().add(panelControl, BorderLayout.EAST);

                this.setVisible(true);

                /* Add all listeners here */

                // Task Management

                btnAddTask.addActionListener(this);

                btnRemoveTask.addActionListener(this);

                btnMoveTaskUp.addActionListener(this);

                btnMoveTaskDown.addActionListener(this);

                btnUpdateTasks.addActionListener(this);

                // Robot Control

                btnF.addActionListener(this);

                btnB.addActionListener(this);

                btnL.addActionListener(this);

                btnR.addActionListener(this);

                btnF.addKeyListener(this);

                btnB.addKeyListener(this);

                btnL.addKeyListener(this);

                btnR.addKeyListener(this);

                btnActivate.addActionListener(this);

                btnActivate.addKeyListener(this);

                cmbRobotNum.addKeyListener(this);

                btnCommand_MoveTo.addActionListener(this);

                btnCommand_MoveAll.addActionListener(this);

                btnCommand_StopTask.addActionListener(this);

                btnCommand_ResumeTask.addActionListener(this);

                btnCommand_ChargeNow.addActionListener(this);

                btnCommand_StopCharging.addActionListener(this);

                // Other

                btnConnect.addActionListener(this);

                btnGetXBeeIDs.addActionListener(this);

                btnAssignID.addActionListener(this);

                panelWebcam.addMouseListener(this);        

                // Set up animation threads

                indicator = new SelectionIndicator(canvas);

                indicator.setRadius(RADIUS+3, 15);  //a tad more than the bot radius

        }

        public void run () {

                while (true) {

                        repaint();

                        try { 

                                Thread.sleep(90);

                        } catch (InterruptedException e) {

                                return;

                        }

                }

        }

        public void paint (Graphics g) {

            super.paint(g);

        }

        public void update (Graphics g) {

            paint(g);

        }

        /** 

        * Gets the JTextArea used for storing the activity log. This method returns a reference to the 

        * JTextArea that stores the log. The log can contain any activity that is revelant to the use

        * of the applet, and may optionally display debugging information.

        *

        * @return the JTextArea where BOM matrix information is stored.

        */ 

        public JTextArea getLog () {

                return log;

        }

        /** 

        * Gets the JTextArea used for storing the BOM matrix data. This method returns a reference to the 

        * JTextArea that stores the BOM matrix. The values in the matrix are stored as integers separated 

        * by spaces, and the lines should be separated by a newline.

        * 

        * @return the JTextArea where BOM matrix information is stored.

        */

        public JTextArea getMatrixInput () {

                return txtMatrix;

        }

        /**

        * Parses a String containing BOM matrix information.

        * The ColonetServerInterface receives lines of the BOM matrix.  (For encoding 

        * information, see the ColonetServerInterface documentation.)  The entire matrix is passed

        * to the client when requested. This method takes a string of the form 

        * "[command code] [command code] [number of robots] [data0] [data1] ..."

        * with tokens separated by spaces and containing no brackets.  

        * The [command code]s are predefined values identifying this String as a BOM data

        * String, [number of robots] is an integer, and the values that follow are 

        * the sensor readings of the robots in order, starting with robot 0.  Only [number of robots]^2

        * data entries will be read.  The matrix values are saved locally until the next String is parsed.

        * 

        *

        * @param line the String containing BOM matrix information.

        * @throws ArrayIndexOutOfBoundsException if there are fewer than [number of robots]^2 data entries in the String

        */

        public void parseMatrix (String line) {

                txtMatrix.setText("");

                String [] str = line.split(" ");

                int num = Integer.parseInt(str[2]);

                for (int i = 0; i < num; i++) {

                        for (int j = 0; j < num; j++) {

                                String next = str[3 + i*num + j];

                                if (next.equals("-1"))

                                        txtMatrix.append("-");

                                else 

                                        txtMatrix.append(next);

                                if (j < num - 1) 

                                        txtMatrix.append(" ");

                        }

                        if (i < num - 1) 

                                txtMatrix.append("\n");

                }

        }

        public void connect () {

            webcamLoader = new WebcamLoader(this);

                dataUpdater = new DataUpdater();

                paintThread = new Thread(this, "paintThread");

                csi = new ColonetServerInterface(this);

                csi.connect(txtHost.getText(), txtPort.getText());

                if (!csi.isReady())

                        return;

                btnConnect.setEnabled(false);

                lblConnectionStatus.setText("Status: Connected");

                //paintThread.start();

                dataUpdater.start();

                webcamLoader.start();

        }

        public void disconnect () {

            btnConnect.setEnabled(true);

            lblConnectionStatus.setText("Status: Disconnected");

            try { paintThread.interrupt(); } catch (Exception e) { }

                try { indicator.interrupt(); } catch (Exception e) { }

        }

        /**

        * Parses a String containing a task queue update.

        * Format is currently not specified.

        * This method currently does nothing.

        *

        * @param line the String containing task queue update information.

        */

        public void parseQueue (String line) {

                log.append("Got queue update\n");

                //TODO: display new queue data in tasks tab

        }

        /**

        * Parses a String containing XBee ID values.

        * The ColonetServerInterface receives Strings of XBee information.  (For encoding 

        * information, see the ColonetServerInterface documentation.)  This method takes

        * a string of the form "[command code] [command code] [number of robots] [id0] [id1] ..."

        * with tokens separated by spaces and containing no brackets.  

        * The [command code]s are predefined values identifying this String as an XBee

        * ID String, [number of robots] is an integer, and the values that follow are 

        * the IDs of the robots in order, starting with robot 0.  Only [number of robots] 

        * will be read.  The ID values are saved locally until the next String is parsed.

        * The purpose of having this list is to ensure that robots are properly identified for control purposes.

        * This keeps robot identification consistent between sessions and prevents arbitrary assignment. 

        *

        * @param line the String containing XBee ID information.

        * @throws ArrayIndexOutOfBoundsException if there are fewer than [number of robots] IDs in the String

        * @see ColonetServerInterface#sendXBeeIDRequest()

        */

        public void parseXBeeIDs (String line) {

                String [] str = line.split(" ");

                int num = Integer.parseInt(str[2]);

                xbeeID = new int[num];

                for (int i = 0; i < num; i++)

                        xbeeID[i] = Integer.parseInt(str[i+3]);

                //update the list of robots to control

                //but save the old value first

                Object oldSelection = cmbRobotNum.getSelectedItem();

                cmbRobotNum.removeAllItems();

                cmbRobotNum.addItem(new String("   All   "));

                for (int i = 0; i < num; i++)

                        cmbRobotNum.addItem(new String("" + xbeeID[i]));

                cmbRobotNum.setSelectedItem(oldSelection);

        }

        /**

        * Parses a String containing battery information.

        * The ColonetServerInterface receives Strings of battery information.  (For encoding 

        * information, see the ColonetServerInterface documentation.)  This method takes

        * a string of the form "[command code] [command code] [robot ID] [value]"

        * with tokens separated by spaces and containing no brackets.  

        * The [command code]s are predefined values identifying this String as a battery

        * information String, [robot ID] is an integer, and [value] is a battery measurement.

        * This updates the batery information for a single robot.

        * 

        *

        * @param line the String containing battery information.

        * @see ColonetServerInterface#sendBatteryRequest(int)

        */

        public void parseBattery (String line) {

                String [] str = line.split(" ");

                int botNum = Integer.parseInt(str[2]);

                int level = Integer.parseInt(str[3]);

                int selected = -1;

                try { 

                    selected = Integer.parseInt((String)cmbRobotNum.getSelectedItem());

                } catch (Exception e) {

                }

                if (selected == botNum) {

                        batteryIcon.setLevel(level);

                }

        }

        /**

        * Parses a String containing visual robot position information along with 

        * canonical ID assignments.

        */

        public void parsePositions (String line) {

                String [] str = line.split(" ");

                java.util.List <RobotIcon> newList = new ArrayList <RobotIcon> ();

                for (int i = 2; i < str.length; i+=3) {

                        int id = Integer.parseInt(str[i]);

                        int x = Integer.parseInt(str[i+1]);

                        int y = Integer.parseInt(str[i+2]);

                        RobotIcon newIcon = new RobotIcon(id, x, y);

                        newList.add(newIcon);

                }

                robotIcons = newList;

        }

        //

        // MouseListener methods

        //

        public void mouseExited(MouseEvent e) {

        }

        public void mouseEntered(MouseEvent e) {

        }

        public void mouseReleased(MouseEvent e) {

        }

        public void mouseClicked(MouseEvent e) {

        }

        public void mousePressed(MouseEvent e) {

            // If we are selecting a waypoint (destination) for a specific bot

            if (setWaypoint && setWaypointID  >= 0) {

                setWaypoint = false;

                panelWebcam.setCursor(new Cursor(Cursor.DEFAULT_CURSOR));

                if (selectedBot < 0)

                    return;

                RobotIcon r = robotIcons.get(selectedBot);

                r.destx = e.getX();

                r.desty = e.getY();

                if (csi != null)

                    csi.sendAbsoluteMove(r.id, r.destx, r.desty);

                return;

            }

            // Right-click also means we are moving a robot

            if (e.getButton() == MouseEvent.BUTTON2 || e.getButton() == MouseEvent.BUTTON3) {

                if (selectedBot < 0)

                    return;

                RobotIcon r = robotIcons.get(selectedBot);

                r.destx = e.getX();

                r.desty = e.getY();

                if (csi != null)

                    csi.sendAbsoluteMove(r.id, r.destx, r.desty);

                return;

            }

            // If we are setting all waypoints

            if (setWaypoint) {

                setWaypoint = false;

                panelWebcam.setCursor(new Cursor(Cursor.DEFAULT_CURSOR));

                for (int i = 0; i < robotIcons.size(); i++) {

                    RobotIcon r = robotIcons.get(i);

                    r.destx = e.getX();

                    r.desty = e.getY();

                }

                return;

            }

            // Otherwise, we are selecting a bot, or doing nothing

                for (int i = 0; i < robotIcons.size(); i++) {

                    RobotIcon r = robotIcons.get(i);

                    if (r.contains(e.getX(), e.getY())) {

                        selectedBot = i;

                        lblSelected.setText(" " + r.id);

                    }

                }

                repaint();

        }

        public void mouseDragged(MouseEvent e) {

        }

        public void mouseMoved(MouseEvent e) {

        }

        //

        // KeyListener methods

        //

        public void keyPressed (KeyEvent e) {

                int code = e.getKeyCode();

                if (code == KeyEvent.VK_UP) {

                        vectorController.setMaxForward();

                        vectorController.sendToServer();

                } else if (code == KeyEvent.VK_DOWN) {

                        vectorController.setMaxReverse();

                        vectorController.sendToServer();

                } else if (code == KeyEvent.VK_LEFT) {

                        vectorController.setMaxLeft();

                        vectorController.sendToServer();

                } else if (code == KeyEvent.VK_RIGHT) {

                        vectorController.setMaxRight();

                        vectorController.sendToServer();

                } else if (code == KeyEvent.VK_S) {

                        vectorController.setZero();

                        vectorController.sendToServer();

                }

        }

        public void keyReleased (KeyEvent e) {

        }

        public void keyTyped (KeyEvent e) {

        }

        //

        // ActionListener method

        //

        public void actionPerformed (ActionEvent e) {

                Object source = e.getSource();

                // General Actions

                if (source == btnConnect) {

                        connect();

                } else if (source == btnGetXBeeIDs) {

                        csi.sendXBeeIDRequest();

                } else if (source == btnAssignID) {

                    String message;

                    if (selectedBot < 0)

                        return;

                    int curID = robotIcons.get(selectedBot).id;

                    if (curID < 0)

                        message = "That robot is unidentified. Please specify its ID.";

                    else

                        message = "That robot has ID " + curID + ". You may reassign it now.";

                    String result = JOptionPane.showInputDialog(this, message, "Robot Identification", JOptionPane.QUESTION_MESSAGE);

                    if (result == null)

                        return;

                int newID = -1;

                    try {

                        newID = Integer.parseInt(result);

                    } catch (Exception ex) {

                        csi.warn("Invalid ID.");

                        return;

                    }

                    // Assign new ID and update display  

                        csi.sendIDAssignment(curID, newID);

                    robotIcons.get(selectedBot).id = newID;

                    robotIcons.get(selectedBot).color = Color.GREEN;

                    lblSelected.setText(" " + newID);

                }

                // Robot Movement Controls

                else if (source == btnF) {

                        vectorController.setMaxForward();

                        vectorController.sendToServer();

                } else if (source == btnB) {

                        vectorController.setMaxReverse();

                        vectorController.sendToServer();

                } else if (source == btnL) {

                        vectorController.setMaxLeft();

                        vectorController.sendToServer();

                } else if (source == btnR) {

                        vectorController.setMaxRight();

                        vectorController.sendToServer();

                } else if (source == btnActivate) {

                        vectorController.setZero();

                        vectorController.sendToServer();

                }

                // Robot Commands (non-movement)

                else if (source == btnCommand_MoveTo) {

                    if (selectedBot < 0)

                        return;

                    panelWebcam.setCursor(new Cursor(Cursor.CROSSHAIR_CURSOR));

                    setWaypoint = true;

                    setWaypointID = selectedBot;

                } else if (source == btnCommand_MoveAll) {

                    panelWebcam.setCursor(new Cursor(Cursor.CROSSHAIR_CURSOR));

                    setWaypoint = true;

                    setWaypointID = -1;

                } else if (source == btnCommand_StopTask) {

                } else if (source == btnCommand_ResumeTask) {

                } else if (source == btnCommand_ChargeNow) {

                } else if (source == btnCommand_StopCharging) {

                }

                // Queue Management

                else if (source == btnAddTask) {

                        taskAddWindow.prompt();

                } else if (source == btnRemoveTask) {

                        if (taskList.getSelectedIndex() >= 0);

                                csi.sendQueueRemove(taskList.getSelectedIndex());

                        csi.sendQueueUpdate();

                } else if (source == btnMoveTaskUp) {

                        csi.sendQueueReorder(taskList.getSelectedIndex(), taskList.getSelectedIndex() - 1);

                        csi.sendQueueUpdate();

                } else if (source == btnMoveTaskDown) {

                        csi.sendQueueReorder(taskList.getSelectedIndex(), taskList.getSelectedIndex() + 1);

                        csi.sendQueueUpdate();

                } else if (source == btnUpdateTasks) {

                        csi.sendQueueUpdate();

                }

        }

        /*

        *        SelectionIndicator thread.

        *        Graphical representation of the selection marker

        *

        *        step() and draw() are synchronized methods. step() is private and 

        *        used to update the position of the crosshairs. draw() is called 

        *        externally and should only run if all calculations in step() have

        *        been completed.

        */

        private class SelectionIndicator extends Thread {

                final int INDICATOR_DELAY = 180;

                final double DTHETA = 0.4;    //larger values make the marker rotate faster

                Graphics2D g;   //canvas to draw on

                boolean running;

                int sx, sy;                //center

                int r, dr;                //radius and width of marker

                double theta;   //current angle

                volatile Polygon poly1, poly2, poly3, poly4;

                int px1, py1;

                int rx1, ry1;

                int px2, py2;

                int rx2, ry2;

                int px3, py3;

                int rx3, ry3;

                int px4, py4;

                int rx4, ry4;

                int steps;

                public SelectionIndicator (Graphics2D g) {

                        super("SelectionIndicator");

                        this.g = g;

                        running = false;

                        steps = 0;

                        theta = 0;

                        rx1 = 0; ry1 = 0;

                        px1 = 0; py1 = 0;

                        rx2 = 0; ry2 = 0;

                        px2 = 0; py2 = 0;

                        rx3 = 0; ry3 = 0;

                        px3 = 0; py3 = 0;

                        rx4 = 0; ry4 = 0;

                        px4 = 0; py4 = 0;

                }

                public synchronized void setCenter (int sx, int sy) {

                        if (sx == this.sx && sy == this.sy) return;

                        this.sx = sx;

                        this.sy = sy;

                        steps = 0;

                }

                public synchronized void setRadius (int r, int dr) {

                        this.r = r;

                        this.dr = dr;

                        steps = 0;

                }

                public void run () {

                        running = true;

                        while (running) {

                                step();

                                try { 

                                        Thread.sleep(INDICATOR_DELAY);

                                } catch (InterruptedException e) {

                                        running = false;

                                        return;

                                }

                        }

                }

                private synchronized void step () {

                        Polygon poly1_new = new Polygon();

                        Polygon poly2_new = new Polygon();

                        Polygon poly3_new = new Polygon();

                        Polygon poly4_new = new Polygon();

                        //the step

                        theta = (theta + DTHETA/Math.PI) % (Math.PI);

                        //the calculation

                        //let p be the point of the pointy thing toward the center

                        //let r be the point at the opposite side

                        //recalculate radius, if it will look cool, lolz

                        int newr = r;

                        if (steps < 100)

                        newr = (int)( r + 200/(steps+1) );

                        //precompute values for dx and dy

                        int dx_inner = (int)(newr * Math.cos(theta));

                        int dy_inner = (int)(newr * Math.sin(theta));

                        int dx_outer = (int)((newr+dr) * Math.cos(theta));

                        int dy_outer = (int)((newr+dr) * Math.sin(theta));

                        //calculate polygon constants

                        int dy_poly = (int)(dr/2 * Math.cos(theta));

                        int dx_poly = (int)(dr/2 * Math.sin(theta));

                        //determine critical points

                        //kansas city shuffle!

                        px1 = sx + dx_inner;

                        py1 = sy - dy_inner;

                        rx1 = sx + dx_outer;

                        ry1 = sy - dy_outer;

                        px2 = sx - dx_inner;

                        py2 = sy + dy_inner;

                        rx2 = sx - dx_outer;

                        ry2 = sy + dy_outer;

                        px3 = sx - dy_inner;

                        py3 = sy - dx_inner;

                        rx3 = sx - dy_outer;

                        ry3 = sy - dx_outer;

                        px4 = sx + dy_inner;

                        py4 = sy + dx_inner;

                        rx4 = sx + dy_outer;

                        ry4 = sy + dx_outer;

                        //create polygons

                        poly1_new.addPoint(px1, py1);

                        poly1_new.addPoint(rx1+dx_poly, ry1+dy_poly);

                        poly1_new.addPoint(rx1-dx_poly, ry1-dy_poly);

                        poly2_new.addPoint(px2, py2);

                        poly2_new.addPoint(rx2+dx_poly, ry2+dy_poly);

                        poly2_new.addPoint(rx2-dx_poly, ry2-dy_poly);

                        poly3_new.addPoint(px3, py3);

                        poly3_new.addPoint(rx3-dy_poly, ry3+dx_poly);

                        poly3_new.addPoint(rx3+dy_poly, ry3-dx_poly);

                        poly4_new.addPoint(px4, py4);

                        poly4_new.addPoint(rx4-dy_poly, ry4+dx_poly);

                        poly4_new.addPoint(rx4+dy_poly, ry4-dx_poly);

                        //reassign updated polygons

                        poly1 = poly1_new;

                        poly2 = poly2_new;

                        poly3 = poly3_new;

                        poly4 = poly4_new;

                        if (steps < 300) steps++;

                }

                public synchronized void draw () {

                        if (!running) return;

                        g.setColor(Color.GRAY);

                        //draw polygons

                        g.fillPolygon(poly1);

                        g.fillPolygon(poly2);

                        g.fillPolygon(poly3);

                        g.fillPolygon(poly4);

                }

        }

        /*

        *        DataUpdater thread.

        *   The purpose of this thread is to request data from the server at regular intervals.

        *

        */

        class DataUpdater extends Thread {

                final int DATAUPDATER_DELAY = 2200;

                public DataUpdater () {

                        super("Colonet DataUpdater");

                }

                public void run () {

                        String line;

                        while (true) {

                                try {

                                        //request more data

                                        if (csi != null && csi.isReady()) {

                        csi.sendPositionRequest();

                                                csi.sendXBeeIDRequest();

                                                if (cmbRobotNum.getSelectedIndex() > 0) {

                                                    String sel = (String) cmbRobotNum.getSelectedItem();

                                                    int num = Integer.parseInt(sel);

                                                        csi.sendBatteryRequest(num);

                                                }

                                        }

                                        Thread.sleep(DATAUPDATER_DELAY);

                                } catch (InterruptedException e) {

                                        return;

                                } 

                        }

                }

        }

        /*

        *        GraphicsPanel class

        *        An extension of JPanel, designed for holding an image that will be repainted regularly.

        */

        class GraphicsPanel extends JPanel {

                protected Image img;

                public GraphicsPanel (Image img) {

                        super();

                        this.img = img;

                }

                public GraphicsPanel (Image img, boolean isDoubleBuffered) {

                        super(isDoubleBuffered);

                        this.img = img;

                }

                public void paint (Graphics g) {

                        // Place the buffered image on the screen, inside the panel

                        g.drawImage(img, 0, 0, Color.WHITE, this);

                }

        }

        /*

        *        WebcamPanel class

        *        Enables more efficient image handling in a component-controlled environment

        */

        class WebcamPanel extends JPanel {

                int BORDER = 16;  // this is arbitrary. it makes the image look nice inside a border.

                int BOT_RADIUS = 40;

                volatile BufferedImage img;

                public WebcamPanel () {

                        super();

                }

                public void setImage (BufferedImage newimg) {

                        if (img != null) {

                                img.flush();

                        }

                        System.gc();

                        img = newimg;

                }

                public void paint (Graphics g) {

                        if (img == null)

                                return;

                        // Place the image on the screen, inside the panel

                        int maxWidth = getWidth() - 2*BORDER;

                        int maxHeight = getHeight() - 2*BORDER;

                        double widthRatio = 1.0 * maxWidth / img.getWidth();

                        double heightRatio = 1.0 * maxHeight / img.getHeight();

                        int newWidth = 0;

                        int newHeight = 0;

                        int x = 0;

                        int y = 0;

                        if (widthRatio > heightRatio) {  //height is the limiting factor

                            newHeight = maxHeight;

                            newWidth = img.getWidth() * maxHeight / img.getHeight();

                            y = BORDER;

                            x = (maxWidth - newWidth) / 2 + BORDER;

                        } else {  //width is the limiting factor

                            newWidth = maxWidth;

                            newHeight = img.getHeight() * maxWidth / img.getWidth();

                            x = BORDER;

                            y = (maxHeight - newHeight) / 2 + BORDER;

                        }

                        Image imgScaled = img.getScaledInstance(newWidth, newHeight, Image.SCALE_FAST);

                        g.drawImage(imgScaled, x, y, this);

                        // Draw Identifiers

                        if (robotIcons == null)

                                return;

                        ((Graphics2D)g).setStroke(new BasicStroke(2));

                        for (int i = 0; i < robotIcons.size(); i++) {

                                RobotIcon r = robotIcons.get(i);

                                g.setColor(r.color);

                                if (widthRatio > heightRatio) {

                                } else {

                                }

                                g.drawOval(r.x-RADIUS, r.y-RADIUS, 2*r.RADIUS, 2*r.RADIUS);

                                // If the robot has a destination, draw the vector

                                if (r.destx >= 0) {

                                    g.drawLine(r.x, r.y, r.destx, r.desty);

                                }

                        }

                        // Identify currently-selected robot

                        if (selectedBot == -1)

                            return;

                        g.setColor(Color.YELLOW);

                        RobotIcon r = robotIcons.get(selectedBot);

                        g.drawOval(r.x-RADIUS-6, r.y-RADIUS-6, 2*r.RADIUS+12, 2*r.RADIUS+12);

                }

        }

        /*

        *        WebcamLoader class

        *        Handles the loading of the webcam image.

        */

        class WebcamLoader extends Thread 

        {

                final int WEBCAMLOADER_DELAY = 300;

                final String IMAGE_PATH = "http://roboclub9.frc.ri.cmu.edu/colonet.jpg";

                URL imagePath;

                MediaTracker mt;

                BufferedImage image;

                Random rand;

                public WebcamLoader (JApplet applet)

                {

                        super("ColonetWebcamLoader");

                        mt = new MediaTracker(applet);

                        ImageIO.setUseCache(false);

                        rand = new Random();

                }

                public void run ()

                {

                        while (true) {

                                try {

                                        Thread.sleep(WEBCAMLOADER_DELAY);

                                        if (image != null) 

                                                image.flush();

                                        System.gc();

                                        try {

                                        imagePath = new URL(IMAGE_PATH + "?rand=" + rand.nextInt(50000));

                                } catch (MalformedURLException e) {

                                        System.out.println("Malformed URL: could not form URL from: [" + IMAGE_PATH + "]\n");

                                }

                                        image = ImageIO.read(imagePath);

                                        // The MediaTracker waitForID pauses the thread until the image is loaded.

                                        // We don't want to display a half-downloaded image.

                                        mt.addImage(image, 1);

                                        mt.waitForID(1);

                                        mt.removeImage(image);

                                        // Save

                                        panelWebcam.setImage(image);

                                } catch (InterruptedException e) {

                                        return;

                                } catch (java.security.AccessControlException e) {

                                        csi.warn("Could not load webcam.\n" + e);

                                        return;

                                } catch (IOException e) {

                                        log.append("IOException while trying to load image.");

                                }

                        }

                }

        }

        /*

        *  RobotIcon class

        *  Provides a means for graphically representing and keeping track of webcam bots.

        */

        class RobotIcon {

                public final int RADIUS = 30;

                public final int CLOSE = 80;

                public int x, y;

                public int destx, desty;

                public int id;

                public Color color;

                public RobotIcon (int id, int x, int y) {

                        this.color = Color.RED;

                        this.x = x;

                        this.y = y;

                        this.id = id;

                        this.destx = -1;

                        this.desty = -1;

                }

                /**

                *  Relocates this RobotIcon to a new coordinate point.

                *

                */

                public void move (int newX, int newY) {

                        this.x = newX;

                        this.y = newY;

                }

                /**

                *  Determines if a given point is within a reasonable range of the current location

                *  to be considered the same robot when moving. The threshold is determined by the 

                *  CLOSE value.

                *

                *  @returns Whether or not the given point is reasonably close to the current location.

                *

                */

                public boolean isClose (int nx, int ny) {

                        int dist = (int) Point.distance(this.x, this.y, nx, ny);

                        return (dist < CLOSE);

                }

                /**

                *  Determines whether a given point is within the rectangle that circumscribes the

                *  robot's circlular icon. Used for clicking on robots in webcam view.

        *

        */

                public boolean contains (int px, int py) {

                    Rectangle rect = new Rectangle(x-RADIUS, y-RADIUS, 2*RADIUS, 2*RADIUS);

                    return rect.contains(px, py);

                }

                public String toString () {

                        String s = "RobotIcon at (" + x + "," + y + "), id " + id;

                        return s;

                }

        }

        /*

        *        VectorController class

        *        Manages robot motion control graphically

        */

        class VectorController extends GraphicsPanel implements MouseListener, MouseMotionListener {

                int x, y, cx, cy;

                int width, height;

                int side;

                public VectorController (Image img) {

                        super (img);

                        width = img.getWidth(null);

                        height = img.getHeight(null);

                        cx = img.getWidth(null)/2;

                        cy = img.getHeight(null)/2;

                        x = cx;

                        y = cy;

                        if (width < height)

                                side = width;

                        else

                                side = height;

                        this.addMouseListener(this);

                        this.addMouseMotionListener(this);

                }

                public void setPoint (int x, int y) {

                        if (!isValidPoint(x, y))

                                return;

                        this.x = x;

                        this.y = y;

                        repaint();

                }

                public boolean isValidPoint (int x, int y) {

                        double xterm = Math.pow(1.0*(x - cx)/(side/2), 2);

                        double yterm = Math.pow(1.0*(y - cy)/(side/2), 2);

                        return (xterm + yterm <= 1);

                }

                public void notifyMouseEvent (MouseEvent e, boolean send) {

                        if (!isValidPoint(e.getX(), e.getY()))

                                return;

                        vectorController.setPoint(e.getX(), e.getY());

                        vectorController.repaint();

                        if (send)

                                vectorController.sendToServer();

                }

                public void mouseExited(MouseEvent e) {

                }

                public void mouseEntered(MouseEvent e) {

                }

                public void mouseReleased(MouseEvent e) {

                        this.notifyMouseEvent(e, true);

                }

                public void mouseClicked(MouseEvent e) {

                        this.notifyMouseEvent(e, false);

                }

                public void mousePressed(MouseEvent e) {

                }

                public void mouseDragged(MouseEvent e) {

                        vectorController.notifyMouseEvent(e, false);

                }

                public void mouseMoved(MouseEvent e) {

                }

                public int getSpeed () {

                        int dx = x - cx;

                        int dy = y - cy;

                        int v = (int) Math.sqrt( Math.pow(dx, 2) + Math.pow(dy, 2) );

                        return v;

                }

                /** 

                * Returns the angle of the control vector in positive degrees west of north,

                * or negative degrees east of north, whichever is less than or equal to

                * 180 degrees total.

                */

                public int getAngle () {

                        int dx = x - cx;

                        int dy = cy - y;

                        // find reference angle in radians

                        double theta = Math.atan2(Math.abs(dx), Math.abs(dy));

                        // transform to degrees

                        theta = theta * 180 / Math.PI;