package nomadgui.state;

import java.awt.*;
import java.awt.geom.*;
import javax.swing.*;
import nomadgui.tools.*;

class SidePanel extends JPanel {
  volatile float theta, phi, alpha, bogie;
  volatile float pan, tilt, field;
  JLabel leftLabel, bogie_left, rightLabel, bogie_right;
  SideImage side;
  Color textColor = new Color(102,102,153);
  
  SidePanel() {
    theta = (float)Math.toRadians(0);
    phi = (float)Math.toRadians(0);
    alpha = (float)Math.toRadians(20);
    bogie = 0;

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

    setLayout(gridbag);

    setPreferredSize(new Dimension(270,270));
    setBorder(BorderFactory.createTitledBorder("Side View"));

    c.gridwidth = 2; c.gridheight = 1;
    c.gridx = 0; c.gridy = 0;
    side = new SideImage();
    gridbag.setConstraints(side, c);
    add(side);
    
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 0; c.gridy = 1;
    leftLabel = new JLabel("Bogie Angle (Left):");
    leftLabel.setHorizontalAlignment(JLabel.CENTER);
    leftLabel.setPreferredSize(new Dimension(200,20));
    gridbag.setConstraints(leftLabel, c);
    add(leftLabel);

    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 1;
    bogie_left = new JLabel(Float.toString(bogie));
    bogie_left.setHorizontalAlignment(JLabel.RIGHT);
    bogie_left.setPreferredSize(new Dimension(50,20));
    bogie_left.setBorder(BorderFactory.createLoweredBevelBorder());
    gridbag.setConstraints(bogie_left, c);
    add(bogie_left);
    
    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 0; c.gridy = 2;
    rightLabel = new JLabel("Bogie Angle (Right):");
    rightLabel.setHorizontalAlignment(JLabel.CENTER);
    rightLabel.setPreferredSize(new Dimension(200,20));
    gridbag.setConstraints(rightLabel, c);
    add(rightLabel);

    c.gridwidth = 1; c.gridheight = 1;
    c.gridx = 1; c.gridy = 2;
    bogie_right = new JLabel(Float.toString(-bogie));
    bogie_right.setHorizontalAlignment(JLabel.RIGHT);
    bogie_right.setPreferredSize(new Dimension(50,20));
    bogie_right.setBorder(BorderFactory.createLoweredBevelBorder());
    gridbag.setConstraints(bogie_right, c);
    add(bogie_right);
  }

  synchronized void setTheta(float t)
    { theta = t; side.setTheta(t); repaint(); }
  synchronized void setPhi(float p)
    { phi = p; side.setPhi(p); repaint(); }
  synchronized void setAlpha(float a)
    { alpha = a; side.setAlpha(a); repaint(); }
  synchronized void setBogie(float b) {
    bogie = b;
    side.setBogie(b);
    if (Math.toDegrees(bogie) >= 20 || Math.toDegrees(bogie) <= -20) {
      leftLabel.setForeground(Color.red);
      bogie_left.setForeground(Color.red);
      rightLabel.setForeground(Color.red);
      bogie_right.setForeground(Color.red);
    } else {
      rightLabel.setForeground(textColor);
      bogie_right.setForeground(textColor);
      leftLabel.setForeground(textColor);
      bogie_left.setForeground(textColor);
    }
    runnableText t1 = new runnableText(Float.toString(Format.round((float)Math.toDegrees(bogie),1)), bogie_left);
    SwingUtilities.invokeLater(t1);

    runnableText t2 = new runnableText(Float.toString(Format.round((float)Math.toDegrees(-bogie),1)), bogie_right);
    SwingUtilities.invokeLater(t2);

    repaint();
  }
  synchronized public void setFOV(float p, float t, float f) {
    pan = p;
    tilt = t;
    field = f;
    side.setFOV(pan,tilt,field);
    repaint();
  }

}

class SideImage extends JPanel {
  Point2D.Float A, B, C, D, E, F, G, H, origin = new Point2D.Float(0,0);
  volatile float theta, phi, alpha, bogie;
  volatile float pan, tilt, field;
  final Color fovColor = new Color(100,100,255,80);        // transparent color
  final Point2D.Float camera = new Point2D.Float(107,96);  // camera position
  Image nomad;
  MediaTracker mt;

  SideImage() {
    setPreferredSize(new Dimension(250,200));

    A = new Point2D.Float(142, 139); // position of A (36 in. off ground, now)
    B = new Point2D.Float(37.5f, 0);  // length of AB
    C = new Point2D.Float(37.5f, 0);  // length of BC
    D = new Point2D.Float(12.5f, 0);  // length of CD
    E = new Point2D.Float(12.5f, 0);  // length of DE
    F = new Point2D.Float(94, 159);  // position of bogie axle
    G = new Point2D.Float(-46, 4);  // distance from F -> G
    H = new Point2D.Float(92, 0);    // length of GH

    theta = (float)Math.toRadians(0);
    phi = (float)Math.toRadians(0);
    alpha = (float)Math.toRadians(20);
    bogie = 0;

    pan = 0;
    tilt = 0;
    field = (float)Math.toRadians(25);

    mt = new MediaTracker(this);
    nomad = Toolkit.getDefaultToolkit().getImage("side.jpg");
    mt.addImage(nomad, 1);
    try { mt.waitForID(1); } catch(InterruptedException ie) {}
  }
  
  synchronized void setTheta(float t) { theta = t; }
  synchronized void setPhi(float p) { phi = p; }
  synchronized void setAlpha(float a) { alpha = a; }
  synchronized void setBogie(float b) { bogie = b; }
  synchronized void setFOV(float p, float t, float f)
    { pan = p;  tilt = t;  field = f; }

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

    AffineTransform at = g2.getTransform();           // get original transform

    // add background image: side view of Nomad
    g2.drawImage(nomad, 0, 0, 250, 200, this);
    
    // ------------------
    // Field of View Cone
    // ------------------

    g2.setColor(fovColor);

    float d1 = Math.abs(89/(float)Math.tan(tilt-.5*field));
    float d2 = Math.abs(89/(float)Math.tan(tilt+.5*field));
    float pd1 = (float)Math.cos(pan) * d1;  // pd1, pd2 are d1 and d2 with pan
    float pd2 = (float)Math.cos(pan) * d2;  // multiplied in for rotation

    if (tilt-.5*field > 0 && tilt+.5*field > 0) {
      g2.setStroke(new BasicStroke(4.0f, BasicStroke.CAP_ROUND,
				   BasicStroke.JOIN_BEVEL));
      float minx, maxx;
      float fl = (float)Math.cos(pan+.5*field) * d1;
      float fr = (float)Math.cos(pan-.5*field) * d1;
      float bl = (float)Math.cos(pan+.5*field) * d2;
      float br = (float)Math.cos(pan-.5*field) * d2;

      // three possible maximum lengths: fl-fr, fl-br, bl-fr
      if (Math.abs(fl-fr) >= Math.abs(fl-br) &&
	  Math.abs(fl-fr) >= Math.abs(bl-fr))      // fl-fr is greatest
	{ minx = (float)camera.x+fl; maxx = (float)camera.x+fr; }
      else if (Math.abs(fl-br) >= Math.abs(fl-fr) &&
	       Math.abs(fl-br) >= Math.abs(bl-fr)) // fl-br is greatest
	{ minx = (float)camera.x+fl; maxx = (float)camera.x+br; }
      else                                         // bl-fr is greatest
	{ minx = (float)camera.x+bl; maxx = (float)camera.x+fr; }
      
      g2.draw(new Line2D.Float(minx, 185, maxx, 185));
    }

    float dash[] = {10, 10};
    g2.setStroke(new BasicStroke(1, BasicStroke.CAP_BUTT,
				 BasicStroke.JOIN_BEVEL, 10, dash, 0));
    if (tilt-.5*field > 0)
      g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			       (float)camera.x+pd1, 185));
    if (tilt+.5*field > 0)
      g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			       (float)camera.x+pd2, 185));
    
    if (tilt-.5*field == 0)
      g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			       400, (float)camera.y));
    if (tilt+.5*field == 0)
      g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			       400, (float)camera.y));
    
    if (tilt-.5*field < 0)
      g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			       (float)camera.x+pd1, 7));  // 7 = 189 - (89*2)
    if (tilt+.5*field < 0)                                // 89=ground->camera
      g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			       (float)camera.x+pd2, 7));
    
    // ----------------
    // Bogie transforms
    // ----------------

    g2.setStroke(new BasicStroke(3.0f));

    // Translate by F
    g2.transform(AffineTransform.getTranslateInstance(F.x, F.y));
    
    // Rotate by (-bogie)
    g2.transform(AffineTransform.getRotateInstance(-bogie));

    // Translate by G
    g2.transform(AffineTransform.getTranslateInstance(G.x, G.y));
    g2.setColor(Color.darkGray);
    g2.fill(new Ellipse2D.Float(-22,-22,44,44));     // Draw wheel at G
    
    // Translate by H
    g2.transform(AffineTransform.getTranslateInstance(H.x, H.y));
    g2.fill(new Ellipse2D.Float(-22,-22,44,44));     // Draw wheel at H
    g2.setColor(Color.blue.darker());
    g2.draw(new Line2D.Float(0, 0, -H.x, H.y));      // Draw GH
    g2.setTransform(at);                              // reset transform

    // Translate by F
    g2.transform(AffineTransform.getTranslateInstance(F.x, F.y));
    
    // Rotate by bogie
    g2.transform(AffineTransform.getRotateInstance(bogie));

    // Translate by G
    g2.transform(AffineTransform.getTranslateInstance(G.x, G.y));
    g2.setColor(Color.blue);
    g2.draw(new Line2D.Float(0, 0, H.x, H.y));       // Draw GH
    g2.setColor(Color.gray.darker());
    g2.fill(new Ellipse2D.Float(-22,-22,44,44));     // Draw wheel at G
    g2.setColor(Color.darkGray);
    g2.fill(new Ellipse2D.Float(-14,-14,28,28));
    g2.setColor(Color.yellow.darker());
    g2.fill(new Ellipse2D.Float(-3,-3,6,6));
    
    // Translate by H
    g2.transform(AffineTransform.getTranslateInstance(H.x, H.y));
    g2.setColor(Color.gray.darker());
    g2.fill(new Ellipse2D.Float(-22,-22,44,44));     // Draw wheel at H
    g2.setColor(Color.darkGray);
    g2.fill(new Ellipse2D.Float(-14,-14,28,28));
    g2.setColor(Color.yellow.darker());
    g2.fill(new Ellipse2D.Float(-3,-3,6,6));
    
    // --------------
    // Arm transforms
    // --------------

    g2.setTransform(at);                              // reset transform
    g2.setColor(Color.black);
    
    // Translate by A
    g2.transform(AffineTransform.getTranslateInstance(A.x, A.y));
    g2.fill(new Ellipse2D.Float(-4,-4,8,8));         // Draw A joint
    B.setLocation(37.5 * Math.sin(theta), 0);         // Find B
    g2.draw(new Line2D.Float(origin, B));            // Draw AB
    
    // Translate by B
    g2.transform(AffineTransform.getTranslateInstance(B.x, B.y));
    g2.fill(new Ellipse2D.Float(-4,-4,8,8));         // Draw B joint
    C.setLocation(-37.5 * Math.sin(theta+phi), 0);    // Find C
    g2.draw(new Line2D.Float(origin, C));            // Draw BC
    
    // Translate by C
    g2.transform(AffineTransform.getTranslateInstance(C.x, C.y));
    g2.fill(new Ellipse2D.Float(-4,-4,8,8));         // Draw C joint

    // Rotate by (-alpha/2 - pi)
    g2.transform(AffineTransform.getRotateInstance(-alpha/2 - Math.PI));
    g2.setStroke(new BasicStroke(2.0f));              // thin lines for wrist
    g2.draw(new Line2D.Float(origin, D));            // Draw CD

    // Translate by D
    g2.transform(AffineTransform.getTranslateInstance(D.x, D.y));
    g2.fill(new Ellipse2D.Float(-3,-3,6,6));         // Draw D joint

    // Rotate by (pi + alpha)
    g2.transform(AffineTransform.getRotateInstance(alpha + Math.PI));
    g2.draw(new Line2D.Float(origin, D));            // Draw DE
    
    // Translate by E
    g2.transform(AffineTransform.getTranslateInstance(D.x, D.y));
    g2.fill(new Ellipse2D.Float(-3,-3,6,6));         // Draw E joint

    // Revert to initial transform
    g2.setTransform(at);
    g2.setStroke(new BasicStroke(1.0f));
  }
  
}