/*
 * NavigationPanel.java
 *
 * Created on January 30, 2003, 5:47 PM
 */

package RobotCmd;

/**
 *
 * @author  sbergbre
 */
public class NavigationPanel extends javax.swing.JPanel {
    
    private static int RADIUS = 20;
    private int _x1;
    private int _y1;
    private int _x2;
    private int _y2;
    private int _theta;
    private int _arcAngle;
    private int _cx;
    private int _cy;
    private int _xTurn;
    private int _yTurn;

    /** Creates new form NavigationPanel */
    public NavigationPanel() {
        navigate(0,0,Math.PI/2,70,70);
        initComponents();
    }
    
    /** Navigate method */
    public void navigate(int x1, int y1, double theta, int x2, int y2) {

        _x1 = x1;
        _y1 = y1;
        _x2 = x2;
        _y2 = y2;
        
        double thetaLine = Math.atan2(y2-y1,x2-x1);
        double thetaDiff = (theta - thetaLine)/2;
        int right = 0;
        if (Math.tan(thetaDiff) > 0)
            right = 1;

        /* Pick point on turning circle where the tangent connects with second point */
        /* Find center of turning circle given a radius of curvature defined by 
         * the Mini-Z Racer */
        double thetaCircle;
        if (right == 1)
            thetaCircle = theta - Math.PI/2;
        else
            thetaCircle = theta + Math.PI/2;

        double dy = RADIUS*Math.sin(thetaCircle);
        double dx = RADIUS*Math.cos(thetaCircle);
        double cx = x1+dx;
        double cy = y1+dy;

        /* Find the orientation of second point w.r.t. center of circle */
        double thetaLine2 = Math.atan2(y2-cy,x2-cx);
        double dLine2 = Math.sqrt(Math.pow((x2-cx),2)+Math.pow((y2-cy),2));
        if (dLine2 < RADIUS)
            return;
        double alpha = Math.acos(RADIUS/dLine2);

        /* Debugging Statements
        System.err.println("ThetaLine = " + thetaLine + "  , ThetaDiff = " + thetaDiff);
        if (right == 1)
            System.err.println("Turning right");
        else
            System.err.println("Turning left");
        System.err.println("ThetaCircle = " + thetaCircle);
        System.err.println("Cx = " + cx + "  , Cy = " + cy);
        System.err.println("dLine2 = " + dLine2);
        System.err.println("ThetaLine2 = " + thetaLine2 + "  , Alpha = " + alpha);
        */
        
        double thetaTurn,arcAngle;
        if (right == 1) {
            theta = theta + Math.PI/2;
            thetaTurn = thetaLine2+alpha;
            if (theta < thetaTurn)
                theta = theta + 2*Math.PI;
            //arcAngle = theta-thetaTurn;
        } else {
            theta = theta - Math.PI/2;
            thetaTurn = thetaLine2-alpha;
            if (thetaTurn < theta)
                thetaTurn = thetaTurn + 2*Math.PI;
            //arcAngle = thetaTurn - theta;
        }
        arcAngle = theta-thetaTurn;

        /*
        System.err.println("ThetaTurn = " + thetaTurn + "  , ArcAngle = " + arcAngle);
        */
        
        _theta = (int)Math.round(Math.toDegrees(theta));
        _arcAngle = (int)Math.round(Math.toDegrees(arcAngle));

        _xTurn = (int)(Math.round(cx+RADIUS*Math.cos(thetaTurn)));
        _yTurn = (int)(Math.round(cy+RADIUS*Math.sin(thetaTurn)));

        _cx = (int)Math.round(cx);
        _cy = (int)Math.round(cy);
        repaint();
    }

    /** Repaint the screen */
    public void paint(java.awt.Graphics g) {
        super.paintComponent(g);
        
        java.awt.Dimension panelSize = new java.awt.Dimension();
        this.getSize(panelSize);
        int panelX = (int)panelSize.getWidth();
        int panelY = (int)panelSize.getHeight();
        int panelCenterX = panelX/2;
        int panelCenterY = panelY/2;
        /* Draw grid */
        g.setColor(java.awt.Color.white);
        g.drawLine(0,panelCenterY,panelX,panelCenterY);
        g.drawLine(panelCenterX,0,panelCenterX,panelY);
        g.translate(panelCenterX, panelCenterY);
        g.drawOval(-2,-2,4,4);
        
        /* Draw significant points on path */
        g.setColor(java.awt.Color.blue);
        g.fillOval(_x1-2,-_y1-2,4,4);
        g.setColor(java.awt.Color.cyan);
        g.fillOval(_xTurn-2,-_yTurn-2,4,4);
        g.setColor(java.awt.Color.red);
        g.fillOval(_x2-2,-_y2-2,4,4);
                
        /* Draw arc and line of robot's path */
        g.setColor(java.awt.Color.green);
        g.drawArc(_cx-RADIUS,-_cy-RADIUS,RADIUS*2,RADIUS*2,_theta,-_arcAngle);
        g.drawLine(_xTurn,-_yTurn,_x2,-_y2);
    }
    
    /** 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

        setLayout(new java.awt.BorderLayout());

        setBackground(new java.awt.Color(0, 0, 0));
        setPreferredSize(new java.awt.Dimension(0, 150));
        setMinimumSize(new java.awt.Dimension(0, 150));
    }//GEN-END:initComponents
    
    
    // Variables declaration - do not modify//GEN-BEGIN:variables
    // End of variables declaration//GEN-END:variables
    
}