package state;

import java.io.*;
import java.awt.*;
import java.awt.event.*;
import java.awt.geom.*;
import javax.swing.*;
import tools.*;

class VitalsPanel extends JPanel {
  JPanel steering, mode, tempPanel, odometer, gpsPanel, timePanel, watchdog;
  WheelPanel wheels;
  GPSPanel gps;
  
  JLabel RSpeed, speed, TurnRadius, turn;          // Labels in steering panel
  JLabel drive, drive_mode, science, science_mode;   // Labels in mode panel
  JLabel InternalTemp, temperature;                  // Labels in temp. panel
  JLabel total, total_dist, trip, trip_dist;         // Labels in odometer
  JLabel X, Y, Z, D, xlabel, ylabel, zlabel, dlabel; // Labels in GPS panel
  JLabel totTime, total_time, curTime, current;      // Labels in time panel

  JCheckBox wdCheck1, wdCheck2, wdCheck3;          // Labels in watchdog panel
  
  Color textColor = new Color(102,102,153);

  volatile int robotSpeed, radius, temp;
  volatile double gps_x, gps_y, gps_z, gps_dist;
  volatile boolean watchdog1, watchdog2, watchdog3;
  volatile long odo_total, odo_trip, time_total, time_trip;
  
  VitalsPanel() {
    // get odometer/time from file
    odo_total = Config.getOdometer();
    time_total = Config.getPower();
    
    // set up panel
    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    setLayout(gridbag);
    c.anchor = GridBagConstraints.NORTHWEST;

    // Steering panel
    c.gridwidth = 1; c.gridheight = 3;
    c.gridx = 0; c.gridy = 0;
    steering = getSteerPanel();
    gridbag.setConstraints(steering, c);
    add(steering);

    // Driving Mode Panel
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 0;
    mode = getModePanel();
    gridbag.setConstraints(mode, c);
    add(mode);

    // Temperature Panel
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 1;
    tempPanel = getTempPanel();
    gridbag.setConstraints(tempPanel, c);
    add(tempPanel);

    // odometer Panel
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 2;
    odometer = getOdometerPanel();
    gridbag.setConstraints(odometer, c);
    add(odometer);

    // GPS Panel
    c.gridwidth = 1; c.gridheight = 2;
    c.gridx = 0; c.gridy = 3;
    gpsPanel = getGPSPanel();
    gridbag.setConstraints(gpsPanel, c);
    add(gpsPanel);

    // Time Panel
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 3;
    timePanel = getTimePanel();
    gridbag.setConstraints(timePanel, c);
    add(timePanel);

    // Watchdog Panel
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 4;
    watchdog = getWatchdogPanel();
    gridbag.setConstraints(watchdog, c);
    add(watchdog);
  }

  synchronized public void setRadius(int rad) {
    radius = rad;
    turn.setText(radius+" m");
    repaint();
  }
  synchronized public void setRobotSpeed(int s) {
    robotSpeed = s;
    if (wheels.negSpeed()) robotSpeed = -robotSpeed;
    speed.setText(robotSpeed+" cm/s");
    repaint();
  }
  synchronized public void setWheelSpeed(int ur, int ul, int lr, int ll) {
    wheels.setSpeed(ur, ul, lr, ll);
    repaint();
  }
  synchronized public void setAmps(boolean ure, boolean ule, boolean lre,
				   boolean lle, boolean sre, boolean sle,
				   boolean urf, boolean ulf, boolean lrf,
				   boolean llf, boolean srf, boolean slf) {
    wheels.setAmps(ure, ule, lre, lle, sre, sle, urf, ulf, lrf, llf, srf, slf);
    repaint();
  }
  synchronized public void setTemp(int f) {
    // temp comes in as 10ths of a degree, farenheit
    temp = (int)((f - 320) * (5.0/9.0)); // convert to 10ths of a deg, celsius
    
    // goes red below 20F (-6.7C) or above 90F (32.2C)
    if (f<=200 || f>=900) temperature.setForeground(Color.red);
    else temperature.setForeground(textColor);
    temperature.setText(temp/10+"."+Math.abs(temp%10)+" C");
    repaint();
  }
  synchronized public void setGPS(double x, double y, double z) {
    gps_dist = Math.sqrt(x*x + y*y);
    if (Math.abs(x)<1000) gps_x = Format.round(x,1);
    else gps_x = Format.round(x/1000, 2);
    if (Math.abs(y)<1000) gps_y = Format.round(y,1);
    else gps_y = Format.round(y/1000, 2);
    if (Math.abs(z)<1000) gps_z = Format.round(z,1);
    else gps_z = Format.round(z/1000, 2);
    gps.setGPS(x, y, z, gps_dist);
    if (Math.abs(gps_dist)<1000) gps_dist = Format.round(gps_dist,1);
    else gps_dist = Format.round(gps_dist/1000, 2);

    xlabel.setText(gps_x+(Math.abs(x)<1000 ? " m" : " km"));
    ylabel.setText(gps_y+(Math.abs(y)<1000 ? " m" : " km"));
    zlabel.setText(gps_z+(Math.abs(z)<1000 ? " m" : " km"));
    dlabel.setText(gps_dist+
		   (Math.abs(Math.sqrt(x*x + y*y))<1000 ? " m" : " km"));
    
    repaint();
  }
  synchronized public void setOdometer(long dist) {
    odo_total += dist; odo_trip += dist;
    total_dist.setText(Double.toString(((double)odo_total)/10000.0));
    trip_dist.setText(Double.toString(((double)odo_trip)/10000.0));
  }
  synchronized public void setTime(long t) {
    time_total += t; time_trip += t;
    
    total_time.setText(parseTime(time_total));
    current.setText(parseTime(time_trip));
  }
  synchronized public void setDriveMode(int m) {
    if (m == 0) drive_mode.setText("Autonomous");
    else if (m == 1) drive_mode.setText("Manual");
    else if (m == 2) drive_mode.setText("Independent");
    else drive_mode.setText("Unknown..");
  }

  private String parseTime(long t) {
    String time;
    long temp = t/1000;            // truncate to a second
    if (temp%60 < 10) time = "0"+temp%60;          // seconds
    else time = Long.toString(temp%60);
    temp/=60;
    if (temp%60 < 10) time = "0"+temp%60+":"+time; // minutes
    else time = temp%60+":"+time;
    temp/=60;
    if (temp%24 < 10) time = "0"+temp%24+":"+time; // hours
    else time = temp%24+":"+time;
    temp/=24;
    time = temp+":"+time;           // days
    return time;
  }
  
  JPanel getSteerPanel() {
    JPanel result = new JPanel();
    
    result.setBorder(BorderFactory.createTitledBorder("Steering"));
    result.setPreferredSize(new Dimension(320,180));

    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    result.setLayout(gridbag);

    c.gridwidth = 1; c.gridheight = 4;
    c.gridx = 0; c.gridy = 0;
    wheels = new WheelPanel();
    gridbag.setConstraints(wheels, c);
    result.add(wheels);

    c.anchor = GridBagConstraints.NORTH;
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 0;
    RSpeed = new JLabel("Robot Speed");
    RSpeed.setHorizontalAlignment(JLabel.LEFT);
    RSpeed.setVerticalAlignment(JLabel.BOTTOM);
    RSpeed.setPreferredSize(new Dimension(100,30));
    gridbag.setConstraints(RSpeed, c);
    result.add(RSpeed);

    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 1;
    speed = new JLabel(robotSpeed+" cm/s");
    speed.setHorizontalAlignment(JLabel.CENTER);
    speed.setPreferredSize(new Dimension(100,20));
    speed.setBorder(BorderFactory.createLoweredBevelBorder());
    gridbag.setConstraints(speed, c);
    result.add(speed);

    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 2;
    TurnRadius = new JLabel("Turn Radius");
    TurnRadius.setHorizontalAlignment(JLabel.LEFT);
    TurnRadius.setVerticalAlignment(JLabel.BOTTOM);
    TurnRadius.setPreferredSize(new Dimension(100,40));
    gridbag.setConstraints(TurnRadius, c);
    result.add(TurnRadius);

    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 3;
    turn = new JLabel(radius+" m");
    turn.setHorizontalAlignment(JLabel.CENTER);
    turn.setPreferredSize(new Dimension(100,20));
    turn.setBorder(BorderFactory.createLoweredBevelBorder());
    gridbag.setConstraints(turn, c);
    result.add(turn);

    return result;
  }

  JPanel getModePanel() {
    JPanel result = new JPanel();
    result.setBorder(BorderFactory.createTitledBorder("Mode"));

    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    result.setPreferredSize(new Dimension(210, 65));
    result.setLayout(gridbag);

    c.anchor = GridBagConstraints.WEST;
    
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 0; c.gridy = 0;
    drive = new JLabel("Driving");
    drive.setPreferredSize(new Dimension(70,20));
    gridbag.setConstraints(drive, c);
    result.add(drive);

    c.gridx = 1;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.gridx = 2; c.gridy = 0;
    drive_mode = new JLabel("Manual");
    drive_mode.setPreferredSize(new Dimension(105,20));
    drive_mode.setHorizontalAlignment(JLabel.CENTER);
    gridbag.setConstraints(drive_mode, c);
    drive_mode.setBorder(BorderFactory.createLoweredBevelBorder());
    result.add(drive_mode);

    c.gridwidth = 1;
    c.gridx = 0; c.gridy = 1;
    science = new JLabel("Science");
    science.setPreferredSize(new Dimension(70,20));
    gridbag.setConstraints(science, c);
    result.add(science);

    c.gridx = 1; c.gridy = 1;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.gridx = 2; c.gridy = 1;
    //science_mode = new JLabel("Acquisition");
    science_mode = new JLabel("Identify");
    science_mode.setPreferredSize(new Dimension(105,20));
    science_mode.setHorizontalAlignment(JLabel.CENTER);
    gridbag.setConstraints(science_mode, c);
    science_mode.setBorder(BorderFactory.createLoweredBevelBorder());
    result.add(science_mode);

    return result;
  }

  JPanel getTempPanel() {
    JPanel result = new JPanel();
    result.setBorder(BorderFactory.createTitledBorder("Temperature"));

    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    result.setPreferredSize(new Dimension(210, 50));
    result.setLayout(gridbag);

    c.anchor = GridBagConstraints.WEST;
    
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 0; c.gridy = 0;
    InternalTemp = new JLabel("Internal");
    InternalTemp.setPreferredSize(new Dimension(70,20));
    gridbag.setConstraints(InternalTemp, c);
    result.add(InternalTemp);

    c.gridx = 1;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.gridx = 2;
    temperature = new JLabel("20.0 C");
    temperature.setPreferredSize(new Dimension(105,20));
    temperature.setHorizontalAlignment(JLabel.CENTER);
    gridbag.setConstraints(temperature, c);
    temperature.setBorder(BorderFactory.createLoweredBevelBorder());
    result.add(temperature);

    return result;
  }

  JPanel getOdometerPanel() {
    JPanel result = new JPanel();
    result.setBorder(BorderFactory.createTitledBorder("Odometer"));

    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    result.setPreferredSize(new Dimension(210, 65));
    result.setLayout(gridbag);

    c.anchor = GridBagConstraints.WEST;

    result.add(Box.createHorizontalStrut(3));
    
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 0; c.gridy = 0;
    total = new JLabel("Total km");
    total.setPreferredSize(new Dimension(70,20));
    gridbag.setConstraints(total, c);
    result.add(total);

    c.gridx = 1;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.gridx = 2;
    total_dist = new JLabel(Double.toString(((double)odo_total)/10000.0));
    total_dist.setPreferredSize(new Dimension(105,20));
    total_dist.setHorizontalAlignment(JLabel.CENTER);
    gridbag.setConstraints(total_dist, c);
    total_dist.setBorder(BorderFactory.createLoweredBevelBorder());
    result.add(total_dist);

    c.gridx = 0; c.gridy = 1;
    trip = new JLabel("Trip km");
    trip.setPreferredSize(new Dimension(70,20));
    gridbag.setConstraints(trip, c);
    result.add(trip);

    c.gridx = 1;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.gridx = 2;
    trip_dist = new JLabel("0");
    trip_dist.setPreferredSize(new Dimension(105,20));
    trip_dist.setHorizontalAlignment(JLabel.CENTER);
    gridbag.setConstraints(trip_dist, c);
    trip_dist.setBorder(BorderFactory.createLoweredBevelBorder());
    result.add(trip_dist);

    return result;
  }

  JPanel getGPSPanel() {
    JPanel result = new JPanel();
    result.setBorder(BorderFactory.createTitledBorder("GPS Coordinates"));

    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    result.setPreferredSize(new Dimension(320, 150));
    result.setLayout(gridbag);

    c.gridwidth = 1; c.gridheight = 5;
    c.gridx = 0; c.gridy = 0;
    gps = new GPSPanel();
    gridbag.setConstraints(gps, c);
    result.add(gps);

    c.anchor = GridBagConstraints.NORTH;
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 0;
    X = new JLabel("N");
    X.setHorizontalAlignment(JLabel.RIGHT);
    X.setPreferredSize(new Dimension(50,20));
    gridbag.setConstraints(X, c);
    result.add(X);

    c.gridx = 1; c.gridy = 1;
    Y = new JLabel("E");
    Y.setHorizontalAlignment(JLabel.RIGHT);
    Y.setPreferredSize(new Dimension(50,20));
    gridbag.setConstraints(Y, c);
    result.add(Y);

    c.gridx = 1; c.gridy = 2;
    Z = new JLabel("Up");
    Z.setHorizontalAlignment(JLabel.RIGHT);
    Z.setPreferredSize(new Dimension(50,20));
    gridbag.setConstraints(Z, c);
    result.add(Z);

    c.gridx = 1; c.gridy = 3;
    result.add(Box.createVerticalStrut(10), c);
    
    c.gridx = 1; c.gridy = 4;
    D = new JLabel("Dist");
    D.setHorizontalAlignment(JLabel.RIGHT);
    D.setPreferredSize(new Dimension(50,20));
    gridbag.setConstraints(D, c);
    result.add(D);

    c.gridx = 2; c.gridy = 0;
    c.gridwidth = 1; c.gridheight = 5;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.anchor = GridBagConstraints.NORTH;
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 3; c.gridy = 0;
    xlabel = new JLabel(gps_x+" m");
    xlabel.setBorder(BorderFactory.createLoweredBevelBorder());
    xlabel.setHorizontalAlignment(JLabel.CENTER);
    xlabel.setPreferredSize(new Dimension(130,20));
    gridbag.setConstraints(xlabel, c);
    result.add(xlabel);

    c.gridx = 3; c.gridy = 1;
    ylabel = new JLabel(gps_y+" m");
    ylabel.setBorder(BorderFactory.createLoweredBevelBorder());
    ylabel.setHorizontalAlignment(JLabel.CENTER);
    ylabel.setPreferredSize(new Dimension(130,20));
    gridbag.setConstraints(ylabel, c);
    result.add(ylabel);

    c.gridx = 3; c.gridy = 2;
    zlabel = new JLabel(gps_z+" m");
    zlabel.setBorder(BorderFactory.createLoweredBevelBorder());
    zlabel.setHorizontalAlignment(JLabel.CENTER);
    zlabel.setPreferredSize(new Dimension(130,20));
    gridbag.setConstraints(zlabel, c);
    result.add(zlabel);

    c.gridx = 3; c.gridy = 3;
    result.add(Box.createVerticalStrut(10), c);
    
    c.gridx = 3; c.gridy = 4;
    dlabel = new JLabel(gps_dist+" m");
    dlabel.setBorder(BorderFactory.createLoweredBevelBorder());
    dlabel.setHorizontalAlignment(JLabel.CENTER);
    dlabel.setPreferredSize(new Dimension(130,20));
    gridbag.setConstraints(dlabel, c);
    result.add(dlabel);

    return result;
  }

  JPanel getTimePanel() {
    JPanel result = new JPanel();
    result.setBorder(BorderFactory.createTitledBorder("Time ON"));

    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    result.setPreferredSize(new Dimension(210, 65));
    result.setLayout(gridbag);

    c.anchor = GridBagConstraints.WEST;
    
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 0; c.gridy = 0;
    totTime = new JLabel("Total");
    totTime.setPreferredSize(new Dimension(70,20));
    gridbag.setConstraints(totTime, c);
    result.add(totTime);

    c.gridx = 1;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.gridx = 2;
    total_time = new JLabel(parseTime(time_total));
    total_time.setPreferredSize(new Dimension(105,20));
    total_time.setHorizontalAlignment(JLabel.CENTER);
    gridbag.setConstraints(total_time, c);
    total_time.setBorder(BorderFactory.createLoweredBevelBorder());
    result.add(total_time);

    c.gridx = 0; c.gridy = 1;
    curTime = new JLabel("Current");
    curTime.setPreferredSize(new Dimension(70,20));
    gridbag.setConstraints(curTime, c);
    result.add(curTime);

    c.gridx = 1;
    result.add(Box.createHorizontalStrut(10), c);
    
    c.gridx = 2;
    current = new JLabel(parseTime(0));
    current.setPreferredSize(new Dimension(105,20));
    current.setHorizontalAlignment(JLabel.CENTER);
    gridbag.setConstraints(current, c);
    current.setBorder(BorderFactory.createLoweredBevelBorder());
    result.add(current);

    return result;
  }

  JPanel getWatchdogPanel() {
    JPanel result = new JPanel();
    checkListen listener = new checkListen();
      
    result.setBorder(BorderFactory.createTitledBorder("Watchdogs"));

    GridBagLayout gridbag = new GridBagLayout();
    GridBagConstraints c = new GridBagConstraints();

    result.setPreferredSize(new Dimension(210, 85));
    result.setLayout(gridbag);

    c.anchor = GridBagConstraints.WEST;
    
    c.gridx = 0; c.gridy = 0;
    c.gridwidth = 1; c.gridheight = 1;
    wdCheck1 = new JCheckBox("Watchdog 1", true);
    wdCheck1.addItemListener(listener);
    wdCheck1.setForeground(textColor);
    wdCheck1.setPreferredSize(new Dimension(200,20));
    gridbag.setConstraints(wdCheck1, c);
    result.add(wdCheck1);

    c.gridx = 0; c.gridy = 1;
    c.gridwidth = 1; c.gridheight = 1;
    wdCheck2 = new JCheckBox("Watchdog 2", true);
    wdCheck2.addItemListener(listener);
    wdCheck2.setForeground(textColor);
    wdCheck2.setPreferredSize(new Dimension(200,20));
    gridbag.setConstraints(wdCheck2, c);
    result.add(wdCheck2);

    c.gridx = 0; c.gridy = 2;
    c.gridwidth = 1; c.gridheight = 1;
    wdCheck3 = new JCheckBox("Watchdog 3", true);
    wdCheck3.addItemListener(listener);
    wdCheck3.setForeground(textColor);
    wdCheck3.setPreferredSize(new Dimension(200,20));
    gridbag.setConstraints(wdCheck3, c);
    result.add(wdCheck3);

    return result;
  }

  class checkListen implements ItemListener {
    public void itemStateChanged(ItemEvent e) {
      watchdog1 = wdCheck1.isSelected();
      watchdog2 = wdCheck2.isSelected();
      watchdog3 = wdCheck3.isSelected();
    }
  }
}

class WheelPanel extends JPanel {
  volatile int URspeed, ULspeed, LRspeed, LLspeed;
  volatile boolean ur_enable, ul_enable, lr_enable, ll_enable, sl_enable,
    sr_enable, ur_fault, ul_fault, lr_fault, ll_fault, sl_fault, sr_fault;
  
  WheelPanel() {
    setPreferredSize(new Dimension(200,150));
    URspeed = ULspeed = LRspeed = LLspeed = 0;
    ur_enable =  ul_enable =  lr_enable =  ll_enable = true;
    ur_fault =  ul_fault =  lr_fault =  ll_fault = false;
    sl_enable = sr_enable = true;
    sl_fault = sr_fault = false;
  }

  public void paintComponent(Graphics g) {
    super.paintComponent(g);
    Graphics2D g2 = (Graphics2D) g;

    g2.setColor(Color.gray);
    g2.fill(new Rectangle2D.Double(20, 20, 130, 85));
    g2.fill(new Ellipse2D.Double(130, 20, 40, 85));

    g2.setColor(Color.blue.darker());
    g2.fill(new Rectangle2D.Double(75, 20, 50, 15));
    g2.fill(new Rectangle2D.Double(75, 90, 50, 15));
    
    g2.setColor(Color.black);
    g2.fill(new RoundRectangle2D.Double(10, 10, 70, 35, 20, 20));
    g2.fill(new RoundRectangle2D.Double(10, 80, 70, 35, 20, 20));
    g2.fill(new RoundRectangle2D.Double(110, 10, 70, 35, 20, 20));
    g2.fill(new RoundRectangle2D.Double(110, 80, 70, 35, 20, 20));

    g2.setColor(Color.white);
    g2.drawString(LLspeed+" cm/s", 20, 25);
    g2.drawString(ULspeed+" cm/s", 120, 25);
    g2.drawString(LRspeed+" cm/s", 20, 95);
    g2.drawString(URspeed+" cm/s", 120, 95);

    g2.setColor(ampColor(ll_enable, ll_fault));
    g2.drawString("AMP", 30, 40);
    g2.setColor(ampColor(ul_enable, ul_fault));
    g2.drawString("AMP", 130, 40);
    g2.setColor(ampColor(lr_enable, lr_fault));
    g2.drawString("AMP", 30, 110);
    g2.setColor(ampColor(ur_enable, ur_fault));
    g2.drawString("AMP", 130, 110);
    g2.setColor(ampColor(sl_enable, sl_fault));
    g2.drawString("AMP", 81, 31);
    g2.setColor(ampColor(sr_enable, sr_fault));
    g2.drawString("AMP", 81, 102);

    g2.setColor(Color.black);
    g2.drawString("Wheel Speeds and", 40, 60);
    g2.drawString("Amp status", 60, 75);

    g2.fill(new Rectangle2D.Double(10, 128, 170, 15));
    g2.setColor(ampColor(true, false));  g2.drawString("enabled", 12, 140);
    g2.setColor(ampColor(false, false)); g2.drawString("disabled", 70, 140);
    g2.setColor(ampColor(false, true));  g2.drawString("faulted", 132, 140);
  }

  Color ampColor(boolean enable, boolean fault) {
    if (fault) return Color.red;
    if (enable) return Color.green;
    return Color.gray;
  }

  public boolean negSpeed() {
    return (URspeed + ULspeed + LRspeed + LLspeed < 0);  // is speed negative?
  }

  synchronized public void setSpeed(int ur, int ul, int lr, int ll) {
    URspeed = ur; ULspeed = ul; LRspeed = lr; LLspeed = ll;
  }
  synchronized public void setAmps(boolean ure, boolean ule, boolean lre,
				   boolean lle, boolean sre, boolean sle,
				   boolean urf, boolean ulf, boolean lrf,
				   boolean llf, boolean srf, boolean slf) {
    ur_enable = ure; ul_enable = ule; lr_enable = lre; ll_enable = lle;
    sl_enable = sle; sr_enable = sre;
    ur_fault = urf; ul_fault = ulf; lr_fault = lrf; ll_fault = llf;
    sl_fault = slf; sr_fault = srf;
  }
    
}

class GPSPanel extends JPanel {
  volatile double x, y, z, dist;
  Point2D.Double origin, end;

  GPSPanel() {
    setPreferredSize(new Dimension(80,90));
    x = y = z = dist = 0;
    origin = new Point2D.Double(40, 40);
    end = new Point2D.Double(40, 40);
  }
  
  synchronized public void setGPS(double xi, double yi, double zi, double d) {
    x = xi;  y = yi;  z = zi;
    dist = d;      // dist is only the x-y distance
    // normalize, make line 20 long
    if (dist == 0)
      end.setLocation(origin);
    else
      end.setLocation((y/dist)*20 + origin.y, -(x/dist)*20 + origin.x);
  }

  public void paintComponent(Graphics g) {
    super.paintComponent(g);
    Graphics2D g2 = (Graphics2D) g;
    setOpaque(true);

    g2.setColor(Color.white);
    g2.fill(new Ellipse2D.Double(10, 10, 60, 60));
      
    g2.setColor(Color.black);

    g2.drawString("N", 36, 8);
    g2.drawString("E", 73, 45);
    
    g2.draw(new Line2D.Double(10, 40, 70, 40));
    g2.draw(new Line2D.Double(40, 10, 40, 70));
    g2.setStroke(new BasicStroke(2.0f));
    g2.draw(new Ellipse2D.Double(10, 10, 60, 60));

    g2.setColor(Color.blue);
    g2.setStroke(new BasicStroke(2.0f));
    g2.draw(new Line2D.Double(origin, end));
    g2.draw(new Ellipse2D.Double(end.x-2, end.y-2, 4, 4));
  }

}