/*
 * Diagnostic.java
 *
 * Created on November 14, 2003, 2:19 PM
 */
package PER;

import PER.rover.*;
import PER.rover.control.*;
import java.awt.image.BufferedImage;
import javax.swing.ImageIcon;
import java.awt.*;

/**
 * Interface program that allows one to operate a rover. Diagnostic.java
 * is useful for testing that a rover is functioning properly because
 * it allows one to access all modes of rover operation including Ackerman
 * driving, Omnidirectional driving, turning, taking pictures, pan, tilt, 
 * and black light control. The Diagnostic program can also be used to
 * teleoperate a rover.
 * <p>
 * Before you can send the rover commands, you must connect to the robot
 * by entering the IP address and hitting Connect. The "Result of last
 * Action:" line will say "Connected" if you have connected successfully.
 * After you have connected, you can turn the rover's UV light on and off
 * by selecting the appropriate radio button, see the current IR reading
 * and battery voltage by hitting the Update button, or watch the rover
 * find a rock by hitting the Find Rock button. The Stop Action button
 * will stop the rover while it is finding a rock. You can also take
 * pictures at different head postions by entering values for Pan and Tilt
 * and then hitting the Take Picture button. Pan values must be between 
 * [-180, 180] with negative values to the right. Tilt values must be
 * between [-50, 90] with negative values meaning down. 
 * <p>
 * You can also drive the robot in different modes. Select the radio
 * button next to Ackerman, Omnidirectional, or Turn and then click
 * in the blue outlined box. Dragging the mouse around inside the box
 * will change the speed and direction of the rover. When you stop
 * pressing the mouse, the rover will stop driving.
 * <p>
 * If you select the Auto-Update toggle button, the rover will take
 * pictures as it drives. This is useful for teleoperation. The rover
 * will face in the direction of travel while driving or forward while turning.
 * At slow speeds the rover's head is angled down. As the speed increases
 * the head angle will increase.
 *
 * @author  Eric Porter
 */
public class Diagnostic extends javax.swing.JFrame {
   
   private Rover rover = null;
   private SendCommands sc;
   private FindRockThread frt = null;
   private boolean UVlight = false;
   private int startx, starty;
   
   private final static int PIC_WIDTH=320, PIC_HEIGHT=240;
   
   private static final int DRIVE_STARTX = 20;
   private static final int DRIVE_STARTY = 30;
   private static final int DRIVE_WIDTH = 260;
   private static final int DRIVE_HEIGHT = 120;
   private static final int PICTURE_DELAY = 5;
   
   /** Creates new form Diagnostic */
   public Diagnostic() {
      initComponents();
      ipTF.setText(Rover.getDefaultIP());
      setTitle("Rover Diagnostic Program");
      
      rover = new Rover();
      sc = new Diagnostic.SendCommands(this, rover);
   }
   
   /** This method is called from within the constructor to
    * initialize the form.
    * WARNING: Do NOT modify this code. The content of this method is
    * always regenerated by the Form Editor.
    */
    private void initComponents() {//GEN-BEGIN:initComponents
        java.awt.GridBagConstraints gridBagConstraints;

        uvButtonGroup = new javax.swing.ButtonGroup();
        driveButtonGroup = new javax.swing.ButtonGroup();
        connectPanel = new javax.swing.JPanel();
        ipLabel = new javax.swing.JLabel();
        ipTF = new javax.swing.JTextField();
        lastActionLabel = new javax.swing.JLabel();
        lastActionFLabel = new javax.swing.JLabel();
        lastResultFLabel = new javax.swing.JLabel();
        lastResultLabel = new javax.swing.JLabel();
        connectButton = new javax.swing.JButton();
        picPanel = new javax.swing.JPanel();
        picLabel = new javax.swing.JLabel();
        takePicButton = new javax.swing.JButton();
        tiltLabel = new javax.swing.JLabel();
        panTF = new javax.swing.JTextField();
        tiltTF = new javax.swing.JTextField();
        panLabel = new javax.swing.JLabel();
        autoUpdate = new javax.swing.JToggleButton();
        uvPanel = new javax.swing.JPanel();
        uvOnButton = new javax.swing.JRadioButton();
        uvButtonGroup.add(uvOnButton);
        uvOffButton = new javax.swing.JRadioButton();
        uvButtonGroup.add(uvOffButton);
        irPanel = new javax.swing.JPanel();
        irLabel = new javax.swing.JLabel();
        updateIRButton = new javax.swing.JButton();
        voltageLabel = new javax.swing.JLabel();
        findRockPanel = new javax.swing.JPanel();
        findRockButton = new javax.swing.JButton();
        stopFindRockButton = new javax.swing.JButton();
        drivePanel = new javax.swing.JPanel();
        ackermanButton = new javax.swing.JRadioButton();
        driveButtonGroup.add(ackermanButton);
        omniButton = new javax.swing.JRadioButton();
        driveButtonGroup.add(omniButton);
        turnButton = new javax.swing.JRadioButton();
        driveButtonGroup.add(turnButton);

        getContentPane().setLayout(new java.awt.GridBagLayout());

        addWindowListener(new java.awt.event.WindowAdapter() {
            public void windowClosing(java.awt.event.WindowEvent evt) {
                exitForm(evt);
            }
        });

        connectPanel.setLayout(new java.awt.GridBagLayout());

        connectPanel.setBorder(new javax.swing.border.LineBorder(new java.awt.Color(0, 0, 0)));
        ipLabel.setText("IP address or hostname: ");
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 0;
        gridBagConstraints.insets = new java.awt.Insets(0, 5, 0, 0);
        connectPanel.add(ipLabel, gridBagConstraints);

        ipTF.setMinimumSize(new java.awt.Dimension(200, 20));
        ipTF.setPreferredSize(new java.awt.Dimension(200, 20));
        ipTF.addActionListener(new java.awt.event.ActionListener() {
            public void actionPerformed(java.awt.event.ActionEvent evt) {
                ipTFActionPerformed(evt);
            }
        });

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 1;
        gridBagConstraints.gridy = 0;
        connectPanel.add(ipTF, gridBagConstraints);

        lastActionLabel.setMinimumSize(new java.awt.Dimension(200, 16));
        lastActionLabel.setPreferredSize(new java.awt.Dimension(200, 16));
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 1;
        gridBagConstraints.gridy = 2;
        gridBagConstraints.insets = new java.awt.Insets(3, 0, 0, 0);
        connectPanel.add(lastActionLabel, gridBagConstraints);

        lastActionFLabel.setText("Last Attempted Action: ");
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 2;
        gridBagConstraints.anchor = java.awt.GridBagConstraints.WEST;
        gridBagConstraints.insets = new java.awt.Insets(3, 5, 0, 0);
        connectPanel.add(lastActionFLabel, gridBagConstraints);

        lastResultFLabel.setText("Result of last Action: ");
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 3;
        gridBagConstraints.anchor = java.awt.GridBagConstraints.WEST;
        gridBagConstraints.insets = new java.awt.Insets(3, 5, 0, 0);
        connectPanel.add(lastResultFLabel, gridBagConstraints);

        lastResultLabel.setMinimumSize(new java.awt.Dimension(200, 16));
        lastResultLabel.setPreferredSize(new java.awt.Dimension(200, 16));
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 1;
        gridBagConstraints.gridy = 3;
        gridBagConstraints.insets = new java.awt.Insets(3, 0, 0, 0);
        connectPanel.add(lastResultLabel, gridBagConstraints);

        connectButton.setText("Connect");
        connectButton.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                connectButtonMouseClicked(evt);
            }
        });

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.gridwidth = 2;
        gridBagConstraints.insets = new java.awt.Insets(3, 5, 0, 0);
        connectPanel.add(connectButton, gridBagConstraints);

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 0;
        gridBagConstraints.gridwidth = 3;
        gridBagConstraints.anchor = java.awt.GridBagConstraints.NORTH;
        getContentPane().add(connectPanel, gridBagConstraints);

        picPanel.setLayout(new java.awt.GridBagLayout());

        picPanel.setBorder(new javax.swing.border.LineBorder(new java.awt.Color(0, 0, 0)));
        picLabel.setMinimumSize(new java.awt.Dimension(320, 240));
        picLabel.setPreferredSize(new java.awt.Dimension(320, 240));
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 0;
        gridBagConstraints.gridwidth = java.awt.GridBagConstraints.REMAINDER;
        picPanel.add(picLabel, gridBagConstraints);

        takePicButton.setText("Take Picture");
        takePicButton.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                takePicButtonMouseClicked(evt);
            }
        });

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.gridheight = java.awt.GridBagConstraints.REMAINDER;
        picPanel.add(takePicButton, gridBagConstraints);

        tiltLabel.setText("Tilt: ");
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 1;
        gridBagConstraints.gridy = 2;
        gridBagConstraints.weightx = 1.0;
        gridBagConstraints.insets = new java.awt.Insets(1, 0, 2, 0);
        picPanel.add(tiltLabel, gridBagConstraints);

        panTF.setText("0");
        panTF.setMaximumSize(new java.awt.Dimension(28, 19));
        panTF.setPreferredSize(new java.awt.Dimension(45, 20));
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 2;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.anchor = java.awt.GridBagConstraints.WEST;
        gridBagConstraints.weightx = 1.0;
        gridBagConstraints.insets = new java.awt.Insets(2, 0, 1, 0);
        picPanel.add(panTF, gridBagConstraints);

        tiltTF.setText("0");
        tiltTF.setMaximumSize(new java.awt.Dimension(28, 19));
        tiltTF.setPreferredSize(new java.awt.Dimension(45, 20));
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 2;
        gridBagConstraints.gridy = 2;
        gridBagConstraints.anchor = java.awt.GridBagConstraints.WEST;
        gridBagConstraints.weightx = 1.0;
        gridBagConstraints.insets = new java.awt.Insets(1, 0, 2, 0);
        picPanel.add(tiltTF, gridBagConstraints);

        panLabel.setText("Pan: ");
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 1;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.insets = new java.awt.Insets(2, 0, 1, 0);
        picPanel.add(panLabel, gridBagConstraints);

        autoUpdate.setFont(new java.awt.Font("Dialog", 1, 11));
        autoUpdate.setText("Auto-Update");
        autoUpdate.setToolTipText("Click to have rover send back images realtime");
        autoUpdate.setMargin(new java.awt.Insets(2, 5, 2, 5));
        autoUpdate.setMaximumSize(new java.awt.Dimension(95, 24));
        autoUpdate.setMinimumSize(new java.awt.Dimension(95, 24));
        autoUpdate.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                autoUpdateMouseClicked(evt);
            }
        });

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 3;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.gridheight = java.awt.GridBagConstraints.REMAINDER;
        picPanel.add(autoUpdate, gridBagConstraints);

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 3;
        gridBagConstraints.gridy = 0;
        gridBagConstraints.gridheight = 3;
        gridBagConstraints.insets = new java.awt.Insets(0, 5, 0, 0);
        gridBagConstraints.anchor = java.awt.GridBagConstraints.NORTHWEST;
        getContentPane().add(picPanel, gridBagConstraints);

        uvPanel.setLayout(new java.awt.GridBagLayout());

        uvPanel.setBorder(new javax.swing.border.LineBorder(new java.awt.Color(0, 0, 0)));
        uvOnButton.setText("UV on");
        uvOnButton.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                uvOnButtonMouseClicked(evt);
            }
        });

        uvPanel.add(uvOnButton, new java.awt.GridBagConstraints());

        uvOffButton.setSelected(true);
        uvOffButton.setText("UV off");
        uvOffButton.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                uvOffButtonMouseClicked(evt);
            }
        });

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 1;
        uvPanel.add(uvOffButton, gridBagConstraints);

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.insets = new java.awt.Insets(5, 0, 0, 0);
        gridBagConstraints.anchor = java.awt.GridBagConstraints.EAST;
        gridBagConstraints.weightx = 1.0;
        getContentPane().add(uvPanel, gridBagConstraints);

        irPanel.setLayout(new java.awt.GridBagLayout());

        irPanel.setBorder(new javax.swing.border.LineBorder(new java.awt.Color(0, 0, 0)));
        irLabel.setText("IR range:");
        irLabel.setPreferredSize(new java.awt.Dimension(155, 16));
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridwidth = 2;
        gridBagConstraints.insets = new java.awt.Insets(0, 5, 0, 0);
        irPanel.add(irLabel, gridBagConstraints);

        updateIRButton.setText("Update");
        updateIRButton.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                updateIRButtonMouseClicked(evt);
            }
        });

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 1;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.insets = new java.awt.Insets(5, 5, 0, 0);
        irPanel.add(updateIRButton, gridBagConstraints);

        voltageLabel.setText("voltage");
        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.insets = new java.awt.Insets(5, 5, 0, 0);
        irPanel.add(voltageLabel, gridBagConstraints);

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 1;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.insets = new java.awt.Insets(5, 5, 0, 0);
        gridBagConstraints.weightx = 1.0;
        getContentPane().add(irPanel, gridBagConstraints);

        findRockPanel.setLayout(new java.awt.GridBagLayout());

        findRockPanel.setBorder(new javax.swing.border.LineBorder(new java.awt.Color(0, 0, 0)));
        findRockButton.setText("Find Rock");
        findRockButton.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                findRockButtonMouseClicked(evt);
            }
        });

        findRockPanel.add(findRockButton, new java.awt.GridBagConstraints());

        stopFindRockButton.setText("Stop Action");
        stopFindRockButton.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mouseClicked(java.awt.event.MouseEvent evt) {
                stopFindRockButtonMouseClicked(evt);
            }
        });

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 1;
        findRockPanel.add(stopFindRockButton, gridBagConstraints);

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 2;
        gridBagConstraints.gridy = 1;
        gridBagConstraints.insets = new java.awt.Insets(5, 5, 0, 0);
        gridBagConstraints.anchor = java.awt.GridBagConstraints.WEST;
        getContentPane().add(findRockPanel, gridBagConstraints);

        drivePanel.setBorder(new javax.swing.border.LineBorder(new java.awt.Color(0, 0, 0)));
        drivePanel.setMinimumSize(new java.awt.Dimension(300, 160));
        drivePanel.setPreferredSize(new java.awt.Dimension(300, 160));
        drivePanel.addMouseListener(new java.awt.event.MouseAdapter() {
            public void mousePressed(java.awt.event.MouseEvent evt) {
                drivePanelMousePressed(evt);
            }
            public void mouseReleased(java.awt.event.MouseEvent evt) {
                drivePanelMouseReleased(evt);
            }
        });
        drivePanel.addMouseMotionListener(new java.awt.event.MouseMotionAdapter() {
            public void mouseDragged(java.awt.event.MouseEvent evt) {
                drivePanelMouseDragged(evt);
            }
        });

        ackermanButton.setSelected(true);
        ackermanButton.setText("Ackerman");
        drivePanel.add(ackermanButton);

        omniButton.setText("Omnidirectional");
        drivePanel.add(omniButton);

        turnButton.setText("Turn");
        drivePanel.add(turnButton);

        gridBagConstraints = new java.awt.GridBagConstraints();
        gridBagConstraints.gridx = 0;
        gridBagConstraints.gridy = 2;
        gridBagConstraints.gridwidth = 3;
        gridBagConstraints.insets = new java.awt.Insets(5, 0, 0, 0);
        getContentPane().add(drivePanel, gridBagConstraints);

        pack();
    }//GEN-END:initComponents

    private void autoUpdateMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_autoUpdateMouseClicked
      
    }//GEN-LAST:event_autoUpdateMouseClicked
   
   private void stopFindRockButtonMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_stopFindRockButtonMouseClicked
      lastActionLabel.setText("Stopping finding the rock");
      if(frt == null || !frt.isAlive()) {
         lastResultLabel.setText("Find Rock not running!");
         return;
      }
      frt.kill();
      lastResultLabel.setText("Success");
   }//GEN-LAST:event_stopFindRockButtonMouseClicked
   
   private void findRockButtonMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_findRockButtonMouseClicked
      lastActionLabel.setText("Finding a rock");
      if(frt != null && frt.isAlive()) {
         lastResultLabel.setText("Find Rock already running!");
         return;
      }
      if(!rover.isConnected()) {
         lastResultLabel.setText("You must connect to the robot first!");
         return;
      }
      frt = new FindRockThread(rover);
      frt.start();
      lastResultLabel.setText("Success");
   }//GEN-LAST:event_findRockButtonMouseClicked
   
   public class FindRockThread extends Thread {
      private Rover rover;
      private boolean stopRunning = false;
      
      public FindRockThread(Rover rov) {
         rover = rov;
      }
      
      public void run() {
         FindRockAction fra = new FindRockAction(false);
         fra.doAction(rover);
         while(!fra.isCompleted()) {
            if(stopRunning)
               fra.kill();
            try{Thread.sleep(20);}catch(Exception e) {}
         }
         if(!fra.isSuccess()) {
            lastResultLabel.setText(ActionConstants.getErrorText(fra.getReturnValue()));
            return;
         }
         if(!fra.hasFoundRock()) {
            lastResultLabel.setText("No rock found");
            return;
         }
         
         ApproachAction aa = new ApproachAction(fra);
         aa.doAction(rover);
         
         while(!aa.isCompleted()) {
            if(stopRunning)
               aa.kill();
            try{Thread.sleep(20);}catch(Exception e) {}
         }
         if(!aa.isSuccess()) {
            lastResultLabel.setText(ActionConstants.getErrorText(aa.getReturnValue()));
            return;
         }
         
         AnalyzeRockAction ara = new AnalyzeRockAction(true, 320, 240);
         ara.doAction(rover);
         while(!ara.isCompleted()) {
            if(stopRunning)
               ara.kill();
            try{Thread.sleep(20);}catch(Exception e) {}
         }
         if(!ara.isSuccess()) {
            lastResultLabel.setText(ActionConstants.getErrorText(ara.getReturnValue()));
            return;
         }
         
         picLabel.setIcon(new ImageIcon(ara.getImage()));
         lastResultLabel.setText("Success");
      }
      
      public void kill() {
         stopRunning = true;
      }
      
   }
   
   private void drivePanelMouseReleased(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_drivePanelMouseReleased
      sc.stopDriving();
   }//GEN-LAST:event_drivePanelMouseReleased
   
   private void drivePanelMouseDragged(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_drivePanelMouseDragged
      coordToAction(evt.getX(), evt.getY());
   }//GEN-LAST:event_drivePanelMouseDragged
   
   private void drivePanelMousePressed(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_drivePanelMousePressed
      coordToAction(evt.getX(), evt.getY());
   }//GEN-LAST:event_drivePanelMousePressed
   
   private void uvOffButtonMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_uvOffButtonMouseClicked
      lastActionLabel.setText("Turning off UV light.");
      if(!rover.isConnected()) {
         lastResultLabel.setText("You must connect to the robot first!");
         uvOnButton.setSelected(UVlight);
         return;
      }
      if(rover.setLight(false))
         UVlight = false;
      
      uvOnButton.setSelected(UVlight);
      redrawIRTF();
      lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
   }//GEN-LAST:event_uvOffButtonMouseClicked
   
   private void uvOnButtonMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_uvOnButtonMouseClicked
      lastActionLabel.setText("Turning on UV light.");
      if(!rover.isConnected()) {
         lastResultLabel.setText("You must connect to the robot first!");
         uvOffButton.setSelected(!UVlight);
         return;
      }
      if(rover.setLight(true))
         UVlight = true;
      
      uvOffButton.setSelected(!UVlight);
      redrawIRTF();
      lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
   }//GEN-LAST:event_uvOnButtonMouseClicked
   
   protected void redrawIRTF() {
      int range = rover.state.getRange();
      int cm = ScanAction.translateScan(range);
      String strcm;
      if(cm == Integer.MAX_VALUE)
         strcm = "off scale";
      else
         strcm = cm+"cm";
      irLabel.setText("IR range: "+strcm+" ("+range+")");
      //I'm printing the voltage to the nearest tenth of a volt
      double voltage = rover.state.getRealVoltage();
      long tenV = Math.round(voltage * 10);
      voltageLabel.setText((tenV/10)+"."+(tenV%10)+"V");
   }
   
   private void updateIRButtonMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_updateIRButtonMouseClicked
      lastActionLabel.setText("Updating IR range.");
      if(!rover.isConnected()) {
         lastResultLabel.setText("You must connect to the robot first!");
         return;
      }
      if(rover.refresh())
         redrawIRTF();
      lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
   }//GEN-LAST:event_updateIRButtonMouseClicked
   
   private void takePicButtonMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_takePicButtonMouseClicked
      lastActionLabel.setText("Taking picture at pan="+panTF.getText()+", tilt="+tiltTF.getText());
      if(!rover.isConnected()) {
         lastResultLabel.setText("You must connect to the robot first!");
         return;
      }
      
      int pan, tilt;
      try{
         pan = Integer.parseInt(panTF.getText());
         if(pan < -180 || pan > 180) {
            lastResultLabel.setText("Error: Pan not in [-180, 180]");
            return;
         }
      }catch(NumberFormatException nfe) {
         lastResultLabel.setText("Error: Pan ang must be an integer!");
         return;
      }
      try{
         tilt = Integer.parseInt(tiltTF.getText());
         if(tilt < -50 || tilt > 90) {
            lastResultLabel.setText("Error: Tilt not in [-50, 90]");
            return;
         }
      }catch(NumberFormatException nfe) {
         lastResultLabel.setText("Error: Tilt angle must be an integer!");
         return;
      }
      BufferedImage image = rover.takePicture(pan, tilt, PIC_WIDTH, PIC_HEIGHT, uvOnButton.isSelected());
      lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
      if(image == null)
         return;
      picLabel.setIcon(new ImageIcon(image));
   }//GEN-LAST:event_takePicButtonMouseClicked
   
   private void takePic(int pan, int tilt) {
      BufferedImage image = rover.takePicture(pan, tilt, PIC_WIDTH, PIC_HEIGHT, false);
      lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
      if(image == null)
         return;
      picLabel.setIcon(new ImageIcon(image));
   }
   
   private void ipTFActionPerformed(java.awt.event.ActionEvent evt) {//GEN-FIRST:event_ipTFActionPerformed
      connectButtonMouseClicked(null);
   }//GEN-LAST:event_ipTFActionPerformed
   
   private void connectButtonMouseClicked(java.awt.event.MouseEvent evt) {//GEN-FIRST:event_connectButtonMouseClicked
      lastActionLabel.setText("Connecting . . .");
      rover.closeComm();
      if(!rover.initComm(ipTF.getText())) {
         lastResultLabel.setText("Unable to Connect");
         rover.closeComm();
         return;
      }
      if(!rover.setLight(uvOnButton.isSelected())) {
         if(rover.state.getStatus() == RoverState.BATTERY_LOW)
            lastResultLabel.setText("Batteries are too low");
         else if(rover.state.getStatus() == RoverState.CEREB_TIMEOUT)
            lastResultLabel.setText("Cerebellum timeout");
         else
            lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
         rover.closeComm();
         return;
      }
      lastResultLabel.setText("Connected");
      String ver = rover.getVersion();
      if(ver != null)
      setTitle("Rover Diagnostic program (rover code v"+ver+")");
      Rover.saveIP(ipTF.getText());
      redrawIRTF();
   }//GEN-LAST:event_connectButtonMouseClicked
   
   /** Exit the Application */
   private void exitForm(java.awt.event.WindowEvent evt) {//GEN-FIRST:event_exitForm
      System.exit(0);
   }//GEN-LAST:event_exitForm
   
   /**
    * @param args the command line arguments
    */
   public static void main(String args[]) {
      //new Diagnostic().show();
       Diagnostic d = new Diagnostic();
       d.show();
       //force the frame to repaint to ensure that joystick grid is in the correct location (was misaligned on some computers)
       try{Thread.sleep(10);}catch(java.lang.InterruptedException e){}
       d.paintAll(d.getGraphics());
   }
   
   public class SendCommands extends Thread {
      private Rover rover;
      private Diagnostic diag;
      
      private static final int STOP = 0;
      private static final int SPIN = 1;
      private static final int CRAB = 2;
      private static final int TURN = 3;
      
      int driveMode = -1;
      int speed = 0, angle=0;
      int radius = 0;
      int picCounter = 1;
 
      public boolean keepRunning = true;
      public SendCommands(Diagnostic d, Rover r) {
         rover = r;
         diag = d;
         this.start();
      }
      
      public void run() {
          
        
        while(keepRunning) {
            //picture taking stuff...too many garshdarn if statements
            //System.out.println(speed);
            if (autoUpdate.isSelected()) { 
               if (picCounter % 5 == 0) {
                    if (speed >= 0)
                        if (driveMode == CRAB)
                            takePic(angle, ((speed - 127) / 3) - 45);
                        else
                           takePic(0, ((speed - 127) / 3) - 45);
                    else
                        if (driveMode == CRAB)
                            if (angle < 0)
                                takePic(angle + 180, (-1 * (speed + 127) / 4) - 32);
                            else 
                                takePic(angle - 180, (-1 * (speed + 127) / 4) - 32);
                        else if (driveMode == SPIN)
                            takePic(0, (-1 * (speed + 127) / 4) - 32);
                        else
                            takePic(-180, (-1 * (speed + 127) / 4) - 32);
                    picCounter = 1;
              }
            }
            switch(driveMode) {
               case STOP: ;
               waitForCommand(); break;
               case SPIN:
                  diag.lastActionLabel.setText("Turning in place.");
                  rover.spin(speed);
                  break;
               case CRAB:
                  diag.lastActionLabel.setText("Omnidirectional at "+angle+" degrees");
                  rover.crab(speed, angle);
                  break;
               case TURN:
                  diag.lastActionLabel.setText("Ackerman at radius "+radius+"cm");
                  rover.quadTurn(speed, radius);
                  break;
               default: waitForCommand();
            }
            picCounter++;
            if(rover.isConnected())
               diag.lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
            diag.redrawIRTF();
            //setStatus();
         }
      }
      
      private synchronized void waitForCommand() {
         if(driveMode == STOP && rover.isConnected()) {
            diag.lastActionLabel.setText("Stopping the rover.");
            rover.setAll(3, 0, 0, 0, 0, 0, 0, 0, 0);
            diag.lastResultLabel.setText(ActionConstants.getErrorText(rover.state.getStatus()));
         }
         try {wait();} catch(InterruptedException e) {}
      }
      
      public synchronized void spin(int pspeed) {
         speed = pspeed;
         driveMode = SPIN;
         notify();
      }
      
      public synchronized void crab(int pspeed, int pang) {
         speed = pspeed;
         angle = pang;
         driveMode = CRAB;
         notify();
      }
      
      public synchronized void turn(int pspeed, int prad) {
         speed = pspeed;
         radius = prad;
         driveMode = TURN;
         notify();
      }
      
      public synchronized void stopDriving() {
         driveMode = STOP;
         notify();
      }
   }
   
   public void paint(Graphics g) {
      super.paint(g);
      startx = DRIVE_STARTX;// + drivePanel.getX();
      starty = DRIVE_STARTY;// + drivePanel.getY();
      g = drivePanel.getGraphics();
      g.setColor(Color.RED);//draw the horizontal line in the middle
      g.drawLine(startx, starty+DRIVE_HEIGHT/2, startx+DRIVE_WIDTH, starty+DRIVE_HEIGHT/2);
      //now draw the box around where it will be legal to joystick the rover in DRIVE mode
      g.setColor(Color.BLUE);
      g.drawLine(startx, starty, startx, starty+DRIVE_HEIGHT);
      g.drawLine(startx+DRIVE_WIDTH, starty, startx+DRIVE_WIDTH, starty+DRIVE_HEIGHT);
      g.drawLine(startx, starty, startx+DRIVE_WIDTH, starty);
      g.drawLine(startx, starty+DRIVE_HEIGHT, startx+DRIVE_WIDTH, starty+DRIVE_HEIGHT);
      g.setColor(Color.BLACK); //draw the center line
      g.drawLine(startx+DRIVE_WIDTH/2, starty, startx+DRIVE_WIDTH/2, starty+DRIVE_HEIGHT);
   }
   
   private void coordToAction(int x, int y) {
      if(x >= startx && x <= startx+DRIVE_WIDTH && y >= starty && y <= starty+DRIVE_HEIGHT) {
         if(!rover.isConnected()) {
            lastActionLabel.setText("Driving");
            lastResultLabel.setText("You must connect to the robot first!");
            return;
         }
         
         // camera updates picture every time it drives
         //takePicButtonMouseClicked(evt);
         
         int speed = starty + DRIVE_HEIGHT/2 - y;
         int speed1 = speed;
         if(speed >= 0)
            speed = 127 + speed*256/DRIVE_HEIGHT;
         else
            speed = speed*256/DRIVE_HEIGHT - 127;
         //System.out.println("speed is "+speed+" x: "+x+" y: "+y+" speed1: "+speed1);
         if(!rover.isConnected())
            return;
         if(ackermanButton.isSelected()) {
            int angle = startx + DRIVE_WIDTH/2 - x;
            double outerAngle = Math.toRadians(84)*angle/DRIVE_WIDTH;
            int radius = (int) Math.round(11.43/Math.tan(outerAngle));
            radius += (radius > 0) ? -12 : 12;
            //System.out.println("the radius is "+(angle == 0 ? 0 : radius)+" speed is "+speed);
            sc.turn(speed, angle == 0 ? 0 : radius);
         }else if(omniButton.isSelected()) {
            int angle = startx + DRIVE_WIDTH/2 - x;
            angle = angle*180/DRIVE_WIDTH;
            sc.crab(speed, angle);
         }else if(turnButton.isSelected())
            sc.spin(speed);
      }
   }
   
    // Variables declaration - do not modify//GEN-BEGIN:variables
    private javax.swing.JRadioButton ackermanButton;
    private javax.swing.JToggleButton autoUpdate;
    private javax.swing.JButton connectButton;
    private javax.swing.JPanel connectPanel;
    private javax.swing.ButtonGroup driveButtonGroup;
    private javax.swing.JPanel drivePanel;
    private javax.swing.JButton findRockButton;
    private javax.swing.JPanel findRockPanel;
    private javax.swing.JLabel ipLabel;
    private javax.swing.JTextField ipTF;
    private javax.swing.JLabel irLabel;
    private javax.swing.JPanel irPanel;
    private javax.swing.JLabel lastActionFLabel;
    private javax.swing.JLabel lastActionLabel;
    private javax.swing.JLabel lastResultFLabel;
    private javax.swing.JLabel lastResultLabel;
    private javax.swing.JRadioButton omniButton;
    private javax.swing.JLabel panLabel;
    private javax.swing.JTextField panTF;
    private javax.swing.JLabel picLabel;
    private javax.swing.JPanel picPanel;
    private javax.swing.JButton stopFindRockButton;
    private javax.swing.JButton takePicButton;
    private javax.swing.JLabel tiltLabel;
    private javax.swing.JTextField tiltTF;
    private javax.swing.JRadioButton turnButton;
    private javax.swing.JButton updateIRButton;
    private javax.swing.ButtonGroup uvButtonGroup;
    private javax.swing.JRadioButton uvOffButton;
    private javax.swing.JRadioButton uvOnButton;
    private javax.swing.JPanel uvPanel;
    private javax.swing.JLabel voltageLabel;
    // End of variables declaration//GEN-END:variables
   
}
