Colony

//

//  Colonet.java

//

import javax.swing.*;

import javax.swing.event.*;

import java.awt.*;

import java.awt.image.*;

import java.awt.event.*;

import java.net.*;

import java.io.*;

import java.util.Random;

import java.applet.*;

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

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

        final int BUFFER = 50;

        final int RADIUS = 30;

        // Connection

        JTextField txtHost;                                

        JTextField txtPort;                                

        JButton btnConnect;        

        JButton btnGraph;

        JLabel lblConnectionStatus;

        JTextArea txtMatrix;

        JTextArea txtInfo; 

        JPanel panelConnect;

        JPanel panelServerInterface;

        // Stats

        JLabel lblBattery;

        JLabel lblTokenPasses;

        JLabel lblHastToken;

        JPanel panelStats;

        // 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;

        VectorController vectorController;

        BufferedImage imageVectorControl;

        JButton btnCommand_StopTask;

        JButton btnCommand_ResumeTask;

        // 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;

        // Graphics

        ImagePanel panel;

        GraphicsConfiguration gc;

        volatile BufferedImage image;

        volatile Graphics2D canvas;

        int cx, cy;

        Socket socket;                                        

        OutputStreamWriter out;

        DataUpdater dataUpdater;  

        Font botFont;

        Random random = new Random();

        volatile int tokenLoc;  //the token is currently here

        volatile int numBots;

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

        volatile Rectangle[] botRect;  //contains boundary shapes around bots for click detection

        volatile int[] xbeeID;

        Thread drawThread;

        Simulator simulator;

        SelectionIndicator indicator;

        PacketMonitor packetMonitor;

        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(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 { drawThread.interrupt(); } catch (Exception e) { }

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

                try { packetMonitor.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

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

                // Calculate center of canvas

                cx = image.getWidth() / 2;

                cy = image.getHeight() / 2;

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

                tokenLoc = 0;

                numBots = 0;

                selectedBot = 0;

                // Connection area

                txtMatrix = new JTextArea();

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

                txtInfo = new JTextArea();

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

                txtInfo.setEditable(false);

                btnGraph = new JButton("Run");

                txtHost = new JTextField("roboclub9.frc.ri.cmu.edu");

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

                txtPort = new JTextField("10123");

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

                btnConnect = new JButton("Connect");

                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(txtInfo);

                panelConnect.add(btnGraph);

                panelServerInterface = new JPanel();

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

                panelServerInterface.add(panelConnect);

                panelServerInterface.add(txtMatrix);

                // Status Elements

                lblTokenPasses = new JLabel();

                lblBattery = new JLabel("???");

                panelStats = new JPanel();

                panelStats.setLayout(new GridLayout(4,2));

                panelStats.add(new JLabel("Token Passes / sec          "));

                panelStats.add(lblTokenPasses);

                panelStats.add(new JLabel("Battery     "));

                panelStats.add(lblBattery);

                panelStats.add(new JLabel("Token Passes / sec     "));

                panelStats.add(lblTokenPasses);

                //TODO: add panelStats somewhere?

                // 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(380, 210);

                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 FlowLayout());

                cmbRobotNum = new JComboBox();

                panelRobotCommands.add(cmbRobotNum);

                panelRobotCommands.add(new JLabel("Commands go here"));

                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);

                // Task Manager Add Window

                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, 150));

                log.setEditable(false);

                // Main control mechanism

                panelControl = new JPanel();

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

                tabPaneControl = new JTabbedPane(JTabbedPane.TOP);

                tabPaneControl.setPreferredSize(new Dimension(400, 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);

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

                this.getContentPane().add(panel, 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);

                btnActivate.addActionListener(this);

                vectorController.addMouseMotionListener(this);

                vectorController.addMouseListener(this);

                // Other

                btnGraph.addActionListener(this);

                btnConnect.addActionListener(this);

                panel.addMouseListener(this);

                this.addMouseMotionListener(this);        

                // Set up dependent threads

                indicator = new SelectionIndicator(canvas);

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

                packetMonitor = new PacketMonitor();

                simulator = new Simulator();

                csi = new ColonetServerInterface(this);

        }

        public synchronized void paint (Graphics g) {

                /*        Redraw the graphical components in the applet. 

                        This paint method overrides the built-in paint of the 

                        JApplet, and we don't want to deal with redrawing the

                        components manually. Fuck that shit. */

                step();

                super.paint(g);

        }

        public synchronized void update (Graphics g) {

                paint(g);

        }

        public void actionPerformed (ActionEvent e) {

                Object source = e.getSource();

                if (source == btnGraph) {

                        btnGraph.setEnabled(false);

                        //Start dependent threads

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

                        drawThread.start();

                        indicator.start();

                        packetMonitor.start();

                        simulator.start();

                } else if (source == btnConnect) {

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

                        dataUpdater = new DataUpdater();

                        dataUpdater.start();

                }

                // 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();

                }

                // 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();

                }

        }

        private void randomize () {

                Random r = new Random();

                StringBuilder s = new StringBuilder();

                int count = r.nextInt(8) + 1;

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

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

                                if (r.nextBoolean())        

                                        s.append("" + (r.nextInt(16) + 1));

                                else 

                                        s.append("-");

                                if (j != count-1)

                                        s.append(" ");

                        }

                        if (i != count-1) s.append("\n");

                }

                txtMatrix.setText(s.toString());

        }

        public void drawRobot (int id, int x, int y) {

                //save the bot in memory, so we can tell if we click on it later

                botRect[id] = new Rectangle(x-RADIUS, y-RADIUS, 2*RADIUS, 2*RADIUS);

                //draw the bot on the canvas

                canvas.setColor(Color.BLACK);

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

                //draw the label

                canvas.setFont(botFont);

                try {

                        canvas.drawString("" + xbeeID[id], x-20, y+2);

                } catch (Exception e) {

                        canvas.drawString("???", x-22, y+2);

                }

        }

        public void drawConnection (int start, int end, int radius, Color color) {

                final int ARROW_LENGTH = 18;

                double angle = 2.0 * Math.PI / numBots;

                int startx, starty, endx, endy;

                startx = cx - (int)(radius * Math.cos(start * angle));

                starty = cy - (int)(radius * Math.sin(start * angle));

                endx = cx - (int)(radius * Math.cos(end * angle));

                endy = cy - (int)(radius * Math.sin(end * angle));

                canvas.setColor(color);

                canvas.drawLine(startx, starty, endx, endy);

                //create arrow

                if (color.equals(Color.BLACK)) return;

                int big_dy = starty - endy;

                int big_dx = endx - startx;

                double theta = 0;

                if (big_dx == 0 && starty > endy) //pointing up

                        theta = Math.PI/2;

                else if (big_dx == 0 && starty < endy) //pointing down 

                        theta = 3*Math.PI/2;

                else if (big_dy == 0 && startx > endx) //pointing left

                        theta = Math.PI;

                else if (big_dy == 0 && startx < endx) //pointing right

                        theta = 0;

                else

                        theta = Math.atan(1.0 * big_dy / big_dx);

                //create ploygon

                Polygon poly = new Polygon();

                int dx_arrow = Math.abs((int)(ARROW_LENGTH * Math.cos(theta)));

                int dy_arrow = Math.abs((int)(ARROW_LENGTH * Math.sin(theta)));

                int dy_half = (int)(ARROW_LENGTH/2 * Math.cos(theta));

                int dx_half = (int)(ARROW_LENGTH/2 * Math.sin(theta));

                int rx = (big_dx > 0) ? endx - dx_arrow : endx + dx_arrow;

                int ry = (big_dy > 0) ? endy + dy_arrow : endy - dy_arrow;

                poly.addPoint(endx, endy);

                poly.addPoint(rx - dx_half, ry - dy_half);

                poly.addPoint(rx + dx_half, ry + dy_half);

                canvas.fillPolygon(poly);

        }

        public void run () {

                while (true) {

                        repaint();

                        try { 

                                Thread.sleep(90);

                        } catch (InterruptedException e) {

                                return;

                        }

                }

        }

        public void step () {

                final int DIAMETER = image.getWidth() - 2*BUFFER;

                final int BIGRADIUS = DIAMETER / 2;

                final int TOKENRADIUS = 40;

                boolean valid;

                // clear image

                canvas.setColor(Color.WHITE);

                canvas.fillRect(0, 0, image.getWidth(), image.getHeight());

                // parse the matrix, to see what robots exist

                String [] rows = txtMatrix.getText().split("\n");

                numBots = rows.length;

                String [][] entries = new String[numBots][numBots];

                valid = true;

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

                        entries[i] = rows[i].split(" ");

                        if (entries[i].length != rows.length) valid = false;

                }

                if (valid) {

                        this.showStatus("Running");

                        // draw robots and find which one is seleced

                        double angle = 2.0 * Math.PI / numBots;

                        canvas.setColor(Color.BLACK);

                        botRect = new Rectangle[numBots];

                        int x, y;

                        if (selectedBot >= numBots) selectedBot = 0;

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

                                x = cx - (int)(BIGRADIUS * Math.cos(i * angle));

                                y = cy - (int)(BIGRADIUS * Math.sin(i * angle));

                                drawRobot(i, x, y);

                                if (i == selectedBot) indicator.setCenter(x, y);

                        }

                        // draw token marker

                        int tokenx, tokeny;

                        int tokenNum = tokenLoc;

                        tokenx = cx - (int)(BIGRADIUS * Math.cos(tokenNum * angle));

                        tokeny = cy - (int)(BIGRADIUS * Math.sin(tokenNum * angle));

                        canvas.setColor(Color.RED);

                        canvas.drawOval(tokenx-TOKENRADIUS, tokeny-TOKENRADIUS, 2*TOKENRADIUS, 2*TOKENRADIUS);

                        // create an inner circle along which the connections are made.

                        // let the diameter of this circle be 2*RADIUS less than the outerDiameter.

                        // see what connections exist

                        for (int row = 0; row < numBots; row++) {

                                for(int col = 0; col < numBots; col++) {

                                        if (!entries[row][col].equals("-") && entries[col][row].equals("-") && row != col) {

                                                //TODO: Make a standard gray

                                                drawConnection(row, col, BIGRADIUS-RADIUS, new Color(200,200,200));

                                        } else if (!entries[row][col].equals("-") && ! entries[col][row].equals("-") && row != col) {

                                                drawConnection(row, col, BIGRADIUS-RADIUS, Color.BLACK);

                                        }

                                }

                        }

                        // draw the selection indicator

                        indicator.draw();

                } else {// if matrix is not valid

                        this.showStatus("Error: Invalid matrix");

                }

        }

        /*        At this point, moveToken is only called by the simulator.

        *        In the future, it can be rewritten to account for non-standard

        *        token passing or deleted if the information can be retrieved

        *        directly from the Colonet server instead.

        */

        public void moveToken () {

                try {

                        tokenLoc = (tokenLoc+1)%numBots;

                } catch (ArithmeticException e) {  // in case numRobots is zero

                }

                packetMonitor.addTokenPass();

        }

        public JTextArea getLog () {

                return log;

        }

        public JTextArea getMatrixInput () {

                return txtMatrix;

        }

        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 parseQueue (String line) {

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

        }

        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

                cmbRobotNum.removeAllItems();

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

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

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

        }

        //

        // MouseEvent methods

        //

        public void mouseExited(MouseEvent e) {

        }

        public void mouseEntered(MouseEvent e) {

        }

        public void mouseReleased(MouseEvent e) {

                vectorController.sendToServer();

        }

        public void mouseClicked(MouseEvent e) {

                mouseDragged(e);

        }

        public void mousePressed(MouseEvent e) {

                try {

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

                                if (botRect[i].contains(e.getPoint()))

                                        selectedBot = i;

                        }

                } catch (Exception ex) {

                        System.out.println(e);

                }

        }

        public void mouseDragged(MouseEvent e) {

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

                vectorController.repaint();

        }

        public void mouseMoved(MouseEvent e) {

        }

        /*

        *        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 = 100;

                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);

                }

        }

        /*

        *        Simulator thread.

        *

        */

        private class Simulator extends Thread {

                final int SIMULATOR_DELAY = 300;

                boolean running;

                public Simulator () {

                        super("Simulator");

                        running = false;

                }

                public void run () {

                        running = true;

                        while (running) {

                                step();

                                try { 

                                        Thread.sleep(SIMULATOR_DELAY);

                                } catch (InterruptedException e) {

                                        running = false;

                                        return; 

                                }

                        }

                }

                private void step () {

                        // don't do anything! the colonet should work on its own!

                }

        }

        /*

        *        PacketMonitor thread.

        *

        *        Currently, this counts the rate of token passes but will eventually 

        *        be modified to keep more important statistics.

        */

        private class PacketMonitor extends Thread {

                final int PACKETMONITOR_DELAY = 1000;

                boolean running;

                int tokenPasses;

                public PacketMonitor () {

                        super("PacketMonitor");

                        running = false;

                        tokenPasses = 0;

                }

                public void run () {

                        running = true;

                        while (running) {

                                displayTokenPasses();

                                try { 

                                        Thread.sleep(PACKETMONITOR_DELAY);

                                } catch (InterruptedException e) { 

                                        running = false;

                                        return; 

                                }

                        }

                }

                public synchronized void addTokenPass () {

                        tokenPasses++;

                }

                public synchronized void displayTokenPasses () {

                        lblTokenPasses.setText("" + tokenPasses);

                        tokenPasses = 0;

                }

        }

        /*

        *        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 = 5000;

                public DataUpdater () {

                        super("Colonet DataUpdater");

                }

                public void run () {

                        String line;

                        while (true) {

                                try {

                                        //request more data

                                        if (csi.isReady()) {

                                                csi.sendSensorDataRequest();

                                                csi.sendXBeeIDRequest();

                                        }

                                        Thread.sleep(DATAUPDATER_DELAY);

                                } catch (InterruptedException e) {

                                        return;

                                } 

                        }

                }

        }

        /*

        *        ImagePanel class

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

        */

        class ImagePanel extends JPanel {

                protected Image img;

                public ImagePanel (Image img) {

                        super();

                        this.img = img;

                }

                public ImagePanel (boolean isDoubleBuffered, Image img) {

                        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);

                }

        }

        /*

        *        VectorController class

        *        Manages robot motion control graphically

        */

        class VectorController extends ImagePanel {

                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;

                }

                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 int getVelocity () {

                        int dx = x - cx;

                        int dy = y - cy;

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

                        return v;

                }

                public int getAngle () {

                        int dx = x - cx;

                        int dy = cy - y;

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

                        theta = (int) (1.0 * theta * 180 / Math.PI);  //transform to degrees

                        if (dy < 0)

                                theta = 180 - theta;

                        theta *= Math.signum(dx);

                        return (int) theta;

                }

                public void paint (Graphics g) {

                        g.setColor(Color.BLACK);

                        g.fillRect(0, 0, width, height);

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

                        g.setColor(Color.RED);

                        g.drawOval(cx-side/2, cy-side/2, side, side);

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

                        g.setColor(Color.GREEN);

                        g.drawLine(cx, cy, x, y);

                        g.fillOval(x-3, y-3, 6, 6);

                }

                public void setMaxForward () {

                        setPoint(cx, cy - (side/2) + 1);

                }

                public void setMaxReverse () {

                        setPoint(cx, cy + (side/2) - 1);

                }

                public void setMaxLeft () {

                        setPoint(cx - (side/2) + 1, cy);

                }

                public void setMaxRight () {

                        setPoint(cx + (side/2) - 1, cy);

                }

                public void setZero () {

                        setPoint(cx, cy);

                }

                public void sendToServer () {

                        if (csi != null)

                                csi.sendData(ColonetServerInterface.MOVE + " " + 

                                        vectorController.getAngle() + " " + 

                                        vectorController.getVelocity(), ColonetServerInterface.GLOBAL_DEST);

                }

        }

        /*

        *        TaskAddWindow class

        *        makes it easy to add tasks to the queue

        */

        class TaskAddWindow extends JFrame implements ActionListener, ListSelectionListener {

                JPanel panelButtons;

                JPanel panelParameters;

                JPanel panelSouth;

                JPanel panelSelection;

                JButton btnSubmit;

                JButton btnCancel;

                DefaultListModel availableListModel;

                JList availableList;

                JScrollPane spAvailableTasks;

                JTextArea txtDescription;

                JTextField txtParameters;

                MouseListener mouseListener;

                public TaskAddWindow () {

                        super("Add a Task ... if you dare!");

                        super.setSize(500,500);

                        super.setLayout(new BorderLayout());

                        // set up buttons

                        btnSubmit = new JButton("Submit");

                        btnCancel = new JButton("Cancel");

                        panelButtons = new JPanel();

                        panelButtons.setLayout(new FlowLayout());

                        panelButtons.add(btnSubmit);

                        panelButtons.add(btnCancel);

                        this.getRootPane().setDefaultButton(btnSubmit);

                        // set up task list

                        availableListModel = new DefaultListModel();

                        availableListModel.addElement("Map the Environment");

                        availableListModel.addElement("Clean Up Chemical Spill");

                        availableListModel.addElement("Grow Plants");

                        availableListModel.addElement("Save the Cheerleader");

                        availableListModel.addElement("Save the World");

                        availableList = new JList(availableListModel);

                        availableList.setSelectionMode(ListSelectionModel.SINGLE_SELECTION);

                        availableList.setSelectedIndex(-1);

                        spAvailableTasks = new JScrollPane(availableList);

                        spAvailableTasks.setBorder(BorderFactory.createTitledBorder("Select A Task"));

                        txtDescription = new JTextArea();

                        txtDescription.setEditable(false);

                        txtDescription.setLineWrap(true);

                        txtDescription.setWrapStyleWord(true);

                        txtDescription.setBorder(BorderFactory.createTitledBorder("Description"));

                        //set up parameter area

                        panelParameters = new JPanel();

                        panelParameters.setLayout(new BorderLayout());

                        txtParameters = new JTextField();

                        panelParameters.add(new JLabel("Optional parameters for this task: "), BorderLayout.WEST);

                        panelParameters.add(txtParameters);

                        // assemble objects

                        panelSelection = new JPanel();

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

                        panelSelection.add(spAvailableTasks);

                        panelSelection.add(txtDescription);

                        panelSouth = new JPanel();

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

                        panelSouth.add(panelParameters);

                        panelSouth.add(panelButtons);

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

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

                        this.setLocationRelativeTo(null);

                        // add listeners here

                        availableList.addListSelectionListener(this);

                        btnSubmit.addActionListener(this);

                        btnCancel.addActionListener(this);

                }

                public void prompt () {

                        this.setVisible(true);

                }

                private String getDescription (int index) {

                        if (index < 0)

                                return "";

                        switch (index) {

                                case 0: return "SLAM and junk";

                                case 1: return "I'm not sure this works";

                                case 2: return "Push them into the light";

                                case 3: return "Watch out for clock repair guys";

                                case 4: return "But make sure he's dead, geez, why would you let him get away? I mean come on, "

                                        +"he's just lying there and everyone's too busy looking at people flying through the sky to "

                                        +"notice? Oh yeah, that's a good transition to a new season, let's make the audience think "

                                        +"he's dead and then pull a fast one on them, they'll never see that coming.";

                                default: return "Task not recognized";

                        }

                }

                public void actionPerformed (ActionEvent e) {

                        Object source = e.getSource();

                        if (source == btnSubmit) {

                                txtParameters.setText(txtParameters.getText().trim());

                                this.setVisible(false);

                        } else if (source == btnCancel) {

                                this.setVisible(false);

                        }

                }

                public void valueChanged (ListSelectionEvent e) {

                        int index = availableList.getSelectedIndex();

                        if (index >= 0)

                                txtDescription.setText(getDescription(index));

                }

        }

}