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

/**
 *
 * @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;
   
   /** Creates new form Diagnostic */
   public Diagnostic() {
      initComponents();
      ipTF.setText(Rover.getDefaultIP());
      setTitle("Rover Diagnostic Program");
      
      rover = new Rover();
      sc = new Diagnostic.SendCommands(this, rover);
      //startx = DRIVE_STARTX + drivePanel.getX();
      //starty = DRIVE_STARTY + drivePanel.getY();
      
      //System.out.println("x: "+drivePanel.getX()+" y: "+drivePanel.getY());
      //System.out.println("width: "+drivePanel.getWidth()+" height: "+drivePanel.getHeight());
   }
   
   /** 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();
      panLabel = new javax.swing.JLabel();
      tiltLabel = new javax.swing.JLabel();
      panTF = new javax.swing.JTextField();
      tiltTF = new javax.swing.JTextField();
      irPanel = new javax.swing.JPanel();
      irLabel = new javax.swing.JLabel();
      updateIRButton = new javax.swing.JButton();
      voltageLabel = new javax.swing.JLabel();
      uvPanel = new javax.swing.JPanel();
      uvOnButton = new javax.swing.JRadioButton();
      uvButtonGroup.add(uvOnButton);
      uvOffButton = new javax.swing.JRadioButton();
      uvButtonGroup.add(uvOffButton);
      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);
      findRockPanel = new javax.swing.JPanel();
      findRockButton = new javax.swing.JButton();
      stopFindRockButton = new javax.swing.JButton();

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

      connectPanel.add(ipTF, new java.awt.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.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.gridwidth = 3;
      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 = 2;
      gridBagConstraints.anchor = java.awt.GridBagConstraints.EAST;
      gridBagConstraints.weightx = 4.0;
      gridBagConstraints.insets = new java.awt.Insets(0, 30, 0, 0);
      picPanel.add(takePicButton, gridBagConstraints);

      panLabel.setText("Pan: ");
      gridBagConstraints = new java.awt.GridBagConstraints();
      gridBagConstraints.gridx = 1;
      gridBagConstraints.gridy = 1;
      gridBagConstraints.anchor = java.awt.GridBagConstraints.EAST;
      gridBagConstraints.weightx = 1.0;
      picPanel.add(panLabel, gridBagConstraints);

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

      panTF.setText("0");
      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(0, 0, 0, 50);
      picPanel.add(panTF, gridBagConstraints);

      tiltTF.setText("0");
      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(0, 0, 0, 50);
      picPanel.add(tiltTF, gridBagConstraints);

      gridBagConstraints = new java.awt.GridBagConstraints();
      gridBagConstraints.gridx = 3;
      gridBagConstraints.gridy = 0;
      gridBagConstraints.gridheight = 3;
      gridBagConstraints.anchor = java.awt.GridBagConstraints.NORTHWEST;
      gridBagConstraints.insets = new java.awt.Insets(0, 5, 0, 0);
      getContentPane().add(picPanel, 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.weightx = 1.0;
      gridBagConstraints.insets = new java.awt.Insets(5, 5, 0, 0);
      getContentPane().add(irPanel, 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.anchor = java.awt.GridBagConstraints.EAST;
      gridBagConstraints.weightx = 1.0;
      gridBagConstraints.insets = new java.awt.Insets(5, 0, 0, 0);
      getContentPane().add(uvPanel, 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);

      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.anchor = java.awt.GridBagConstraints.WEST;
      gridBagConstraints.insets = new java.awt.Insets(5, 5, 0, 0);
      getContentPane().add(findRockPanel, gridBagConstraints);

      pack();
   }//GEN-END:initComponents
   
   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 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");
      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();
   }
   
   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;
      
      public boolean keepRunning = true;
      public SendCommands(Diagnostic d, Rover r) {
         rover = r;
         diag = d;
         this.start();
      }
      
      public void run() {
         while(keepRunning) {
            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();
            }
            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;
         }
         
         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.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
   
}
