package state;

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

public class TopView extends JPanel {
  
  //-------------------------------------------------//
  // Note on distances/positions                     //
  //                                                 //
  // The top/side images of the robot are 2cm to 1   //
  // pixel.  All measurements in this file are in    //
  // inches, if they are followed by (* 1.27).  This //
  // is the conversion between inches and pixels.    //
  //-------------------------------------------------//

  // Variables for the steering links
  volatile private double theta;                  // angle of wheel rotation
  volatile private double gr, gl;                 // G right and left (y value)
  volatile private double AB, BC, BE, CD, CG, EF, EW;     // link lengths
  volatile private Point2D.Double A, B, C, D, E, F, G, W; // Wheel points
  volatile private Point2D.Double UR, UL, LR, LL; // 'A' pos. for each wheel
  volatile private Wheel wheel;                   // The wheels

  // Variables for the arm
  volatile private Point2D.Double armA, armB, armC, orig; // points on the arm
  volatile private double armTheta, phi;                  // arm angles

  // Variables for the Field of View cone
  volatile private double pan, tilt, field;         // pan, tilt, and FOV angle
  final Color fovColor = new Color(100,100,255,80);        // transparent color
  final Point2D.Double camera = new Point2D.Double(107,100); // camera position
  
  // Variables for the window/drawing
  final static BasicStroke thin = new BasicStroke(2.0f);  // thin drawing line
  final static BasicStroke wide = new BasicStroke(4.0f);  // wide drawing line

  public TopView() {
    setPreferredSize(new Dimension(250,240));

    // link lengths
    AB =  15.0 * 1.27;
    BC =  11.0 * 1.27;
    BE =   4.0 * 1.27;
    CD =  18.0 * 1.27;
    CG =  35.0 * 1.27;
    EF =  20.0 * 1.27;
    EW =  10.5 * 1.27;
    
    // Reference points... may also need to change lengths
    // above to change link sizes
    A = new Point2D.Double(0, 0);
    B = new Point2D.Double();
    C = new Point2D.Double();
    D = new Point2D.Double(21.25 * 1.27, 28.25 * 1.27);
    E = new Point2D.Double();
    F = new Point2D.Double();
    G = new Point2D.Double(-0.25 * 1.27, 60.39 * 1.27);  // stowed
    W = new Point2D.Double();

    // Absolute points: positions for 'A' for each wheel
    UR = new Point2D.Double(137, 149);
    UL = new Point2D.Double(137, 120);
    LR = new Point2D.Double(52, 149);
    LL = new Point2D.Double(52, 120);

    gr = 76.7; gl = 76.7;   // stowed

    // Arm points
    armA = new Point2D.Double(140, 145);  // Absolute position, arm start point
    armB = new Point2D.Double(37.5, 0);   // length of AB
    armC = new Point2D.Double(37.5, 0);   // length of BC
    orig = new Point2D.Double(0, 0);      // origin
    theta = 0;  phi = 0;                  // arm angles

    // FOV variables
    pan = 0;
    tilt = 0;
    field = Math.toRadians(25);
    
    repaint();
  }

  synchronized public void setLinks(double l, double r) {
    // l,r in mm... mm/10 = cm, cm/2 = pixels ==> mm/20 = pixels
    if (l<85 || r<85 || l>692 || r>692) {
      System.out.println("ERROR: Telemetry for steering "+
			 "giving incorrect values (l="+l+", r="+r+")");
      gl = 927/20;  gr = 927/20;  // set to safe value
    } else {
      gl = (l+875)/20;  // +875 compensates for 87.5 cm difference in
      gr = (r+875)/20;  // the reference points of telemetry and the steering
      if (gl > 76.7) gl = 76.7;
      if (gr > 76.7) gr = 76.7;
    }
    repaint();
  }
  synchronized public void setTheta(double t) { armTheta = t;  repaint(); }
  synchronized public void setPhi(double p) { phi = p; repaint(); }
  synchronized public void setFOV(double p, double t, double f) {
    pan = p / 100.0;
    tilt = t / 100.0;
    field = f / 100.0;
    repaint();
  }

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

    setOpaque(true);
    setBackground(Color.white);

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

    // ---------------------------------
    // Draw steering linkages and wheels
    // ---------------------------------
    
    // Translate to Upper Right A point, rotate sideways
    g2.transform(AffineTransform.getTranslateInstance(UR.x, UR.y));
    g2.transform(AffineTransform.getRotateInstance(Math.PI/2));
    drawLink(at, g2, true, true);
   
    // Translate to Upper Left A point
    g2.transform(AffineTransform.getTranslateInstance(UL.x, UL.y));
    g2.transform(AffineTransform.getRotateInstance(Math.PI/2));
    drawLink(at, g2, false, true);
   
    // Translate to Lower Right A point
    g2.transform(AffineTransform.getTranslateInstance(LR.x, LR.y));
    g2.transform(AffineTransform.getRotateInstance(Math.PI/2));
    drawLink(at, g2, true, false);

    // Translate to Lower Left A point
    g2.transform(AffineTransform.getTranslateInstance(LL.x, LL.y));
    g2.transform(AffineTransform.getRotateInstance(Math.PI/2));
    drawLink(at, g2, false, false);

    // ------------------
    // Draw Field of View
    // ------------------
    
    g2.setColor(fovColor);

    // Find distances to viewing area, add camera coords to make
    //   the points use global coordinates.
    // 89 is the height between the ground and camera (in pixels)
    // Points are arranged like this:
    //   fl _______ fr     +x is up, +y is right... screen coords
    //      \     /        this field is drawn facing this way =>
    //       \___/         so that fl and fr are on the right edge,
    //     bl     br       and bl and br are on the left edge
    
    float d1 = Math.abs(89/(float)Math.tan(tilt-.5*field));
    float d2 = Math.abs(89/(float)Math.tan(tilt+.5*field));
    float x1 = Math.abs((89/(float)Math.sin(tilt-.5*field))
      *(float)Math.tan(.5*field));
    float x2 = Math.abs((89/(float)Math.sin(tilt+.5*field))
      *(float)Math.tan(.5*field));

    Point2D.Float fl = new Point2D.Float((float)camera.x+d1,
					 (float)camera.y-x1);
    Point2D.Float fr = new Point2D.Float((float)camera.x+d1,
					 (float)camera.y+x1);
    Point2D.Float bl = new Point2D.Float((float)camera.x+d2,
					 (float)camera.y-x2);
    Point2D.Float br = new Point2D.Float((float)camera.x+d2,
					 (float)camera.y+x2);
    
    GeneralPath view = new GeneralPath();      // Set up path
    view.moveTo(bl.x, bl.y);
    view.lineTo(br.x, br.y);
    view.lineTo(fr.x, fr.y);
    view.lineTo(fl.x, fl.y);
    view.lineTo(bl.x, bl.y);
    view.closePath();

    // Rotate to correct position (around camera center)
    g2.transform(AffineTransform.getRotateInstance(-pan, camera.x, camera.y));
    if (tilt > 0) g2.fill(view);               // Draw the field

    // Draw the dashed lines displaying the field of view edges
    float dash[] = {10, 10};
    g2.setStroke(new BasicStroke(1, BasicStroke.CAP_BUTT,
				 BasicStroke.JOIN_BEVEL, 10, dash, 0));
    g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			     (fl.x+bl.x)/2, (fl.y+bl.y)/2));
    g2.draw(new Line2D.Float((float)camera.x, (float)camera.y,
			     (fr.x+br.x)/2, (fr.y+br.y)/2));
	    
    g2.setTransform(at);                       // reset transform
    
    // --------
    // Draw arm
    // --------

    g2.setColor(Color.black);

    // Translate to Arm start point (armA)
    g2.transform(AffineTransform.getTranslateInstance(armA.x, armA.y));
    g2.fill(new Ellipse2D.Double(-4,-4,8,8));  // draw A joint
    g2.setStroke(new BasicStroke(3.0f));       // change to arm width

    // Rotate to put x-axis on first arm link
    g2.transform(AffineTransform.getRotateInstance(armTheta-Math.PI/2));
    g2.draw(new Line2D.Double(orig, armB));    // draw link AB

    // Translate to armB
    g2.transform(AffineTransform.getTranslateInstance(armB.x, armB.y));
    g2.fill(new Ellipse2D.Double(-4,-4,8,8));  // draw B joint

    // Rotate to put x-axis on second arm link
    g2.transform(AffineTransform.getRotateInstance(-(Math.PI - phi)));
    g2.draw(new Line2D.Double(orig, armC));    // draw link BC

    // Translate to armC
    g2.transform(AffineTransform.getTranslateInstance(armC.x, armC.y));
    g2.fill(new Ellipse2D.Double(-4,-4,8,8));  // draw C joint
    
    g2.setTransform(at);                       // reset transform
  }

  void flipX() {
    A.x = -A.x;  B.x = -B.x;  C.x = -C.x;  D.x = -D.x;
    E.x = -E.x;  F.x = -F.x;  G.x = -G.x;  W.x = -W.x;
    theta = -theta;
  }
  
  void flipY() {
    A.y = -A.y;  B.y = -B.y;  C.y = -C.y;  D.y = -D.y;
    E.y = -E.y;  F.y = -F.y;  G.y = -G.y;  W.y = -W.y;
    theta = -theta;
  }
  
  private void drawLink(AffineTransform at, Graphics2D g2,
			boolean right, boolean top) {
    calcPositions(right, top);
    
    g2.setColor(Color.gray);
    g2.setStroke(wide);
    //g2.setColor(Color.red);
    g2.draw(new Line2D.Double(A, B));
    g2.setColor(Color.yellow.darker());
    g2.draw(new Line2D.Double(E, F));
    g2.setColor(Color.gray);
    g2.draw(new Line2D.Double(E, C));
    //g2.setColor(Color.blue);
    g2.draw(new Line2D.Double(C, D));

    g2.setStroke(thin);
    g2.draw(new Line2D.Double(G, C));

    if (right) wheel = new Wheel(W.x - 11, W.y - 22, 20, 44);
    else wheel = new Wheel(W.x - 8, W.y - 22, 20, 44);
    
    GradientPaint tread = wheel.setTread((float)wheel.x, (float)wheel.y,
					 Color.lightGray, (float)wheel.x,
					 (float)wheel.y +
					 (((float)wheel.height)/10),
					 Color.black, true);
    g2.setPaint(tread);
    g2.transform(AffineTransform.getRotateInstance(theta, W.x, W.y));
    g2.fill(wheel);
    g2.setPaint(Color.darkGray);
    if (right) g2.fill(new Rectangle2D.Double(W.x+9, W.y-15, 3, 30));
    else g2.fill(new Rectangle2D.Double(W.x-11, W.y-15, 3, 30));
    g2.setPaint(Color.blue);

    g2.setTransform(at);                     // reset transform
  }

  synchronized private void calcPositions(boolean right, boolean top) {
    G.y = right ? gr : gl;

    A.setLocation(0, 0);
    D.setLocation(21.25 * 1.27, 28.25 * 1.27);
    G.setLocation(-0.25 * 1.27, (right ? gr : gl));

    // all angles: <Angle point>_ang_<triangle>
    //   i.e., D_ang_CGD is angle D on triangle CGD
    // an 'R' means right triangle with the two other points
    //   forming the hypotenuse of the triangle
    //   i.e., G_ang_DGR is angle G in the right
    //     triangle with hypotenuse DG

    // Find C
    double DG = D.distance(G);
    double D_ang_CGD = Math.acos((CG*CG - CD*CD - DG*DG) / (-2*CD*DG));
    double G_ang_DGR = Math.acos((D.x-G.x) / DG);
    C.x = D.x - CD * Math.cos(D_ang_CGD - G_ang_DGR);
    C.y = D.y - CD * Math.sin(D_ang_CGD - G_ang_DGR);

    // Find B
    double AC = A.distance(C);
    double A_ang_ABC = Math.acos((BC*BC - AB*AB - AC*AC) / (-2*AB*AC));
    double A_ang_ACR = Math.acos((C.y-A.y) / AC);
    B.x = A.x + AB * Math.sin(A_ang_ABC + A_ang_ACR);
    B.y = A.y + AB * Math.cos(A_ang_ABC + A_ang_ACR);

    // Find E
    E.x = ((BC+BE)/BC)*(B.x-C.x) + C.x;
    E.y = C.y - ((BC+BE)/BC)*(C.y-B.y);

    // Find theta (wheel rotation and angle of link)
    theta = Math.asin((E.x-B.x)/BE);
    //System.out.println("theta = "+theta+" at G.y = "+G.y);
    
    // Find F
    F.y = E.y + EF*((E.x-B.x)/BE);  // (E.x-B.x)/BE = sin E_ang_EBR
    F.x = E.x + EF*Math.cos(theta);

    // Find W
    W.y = E.y + EW*((E.x-B.x)/BE);  // (E.x-B.x)/BE = asin E_ang_EBR
    W.x = E.x + EW*Math.cos(theta);
    
    if(!right) flipX();
    if(!top) flipY();

//     System.out.println((top?"Upper ":"Lower ")+
// 		       (right?"right ":"left ")+"wheel");

//     System.out.println("A: "+A);
//     System.out.println("B: "+B);
//     System.out.println("C: "+C);
//     System.out.println("D: "+D);
//     System.out.println("E: "+E);
//     System.out.println("F: "+F);
//     System.out.println("G: "+G);
//     System.out.println("W: "+W);

  }
}