// Kine.java  from CH 3 S&N Autonomous Mobile Robots
// given starting position, goal is origin, control vector
// navagate to goal. From any x0,y0 to x1,y1 OK, just compute dx,dy
// rotate entire frame of reference for final theta other than 0.0 

// plot with x1,y1 at 0,0

import java.*;
import java.awt.*;
import java.awt.event.*;

public class Kine extends Frame
{
  // chosen control parameters
  double kr = 3.0, ka = 8.0, kb = -3.0; // was -1.5;
  // chosen starting point
  double xi0 =  0.5, yi0 =  0.5; // from
  double xi1 = -0.5, yi1 = -0.5; // to in unit box 
  
  double t = 0.0, dt = 0.01;
  double tstep = 0.01, tlast = 0.0; // try 100 step per sec 
  double dx, dy;
  double alpha, beta, theta, dalpha, dbeta, dtheta;
  double rho, drho;
  double scale = 180.0;
  int cycle = 0;
  boolean done = false;

  Kine()
  {
    setTitle("Kine");
    setSize(500,500);
    setBackground(Color.white);
    setForeground(Color.black);
    addWindowListener(new WindowAdapter()
    {
      public void windowClosing(WindowEvent e)
      {
        System.exit(0);
      }
    });
    setVisible(true);
    init();
    new MyThread().start();
  }
   
  class MyThread extends Thread // for movement computation
  {
    MyThread()
    {          // everything in "run"
    }
    
    public void run()
    {
      while(true)
      {
        try
        {
          physicsComputation();
          // System.out.println(id+" thread is running"); // debug
          sleep(50); // milliseconds delay 
        }
        catch(InterruptedException e)
        {
          System.out.println("interruptedException");
        }
      }
    }
  }

  void init()
  {
    System.out.println("Kine.java starting");
    dx = xi1-xi0; // delta position to origin
    dy = yi1-yi0;
    rho = Math.sqrt(dx*dx+dy*dy);
    alpha = Math.PI/8.0;  // angle robot X to goal
    beta  = -Math.PI/4.0; // angle robot cg to goal
    theta = -(beta+alpha);  // angle robot X to inertial
  } // end init

  int p(double x) // simple transformation on a point
  {
    double offset = 250.0;
    double scale  = 1.0;
    return (int)(x*scale + offset);
  }

  int q(double y) // simple transformation on a point
  {
    double offset = 250.0;
    double scale  = 1.0;
    return (int)(offset-y*scale);
  }
  
  double px(double x, double y) // rotation and translation
  {
    double offsetx = -dx*scale;
    double r, a;
    r = Math.sqrt(x*x+y*y);
    a = Math.atan2(y,x);
    return r*Math.cos(a+theta) + offsetx;
  }

  double py(double x, double y) // simple transformation on a point
  {
    double offsety = -dy*scale;
    double r, a;
    r = Math.sqrt(x*x+y*y);
    a = Math.atan2(y,x);
    return r*Math.sin(a+theta) + offsety;
  }

  void physicsComputation()
  {
    double tnow; // for drawing timing control
    
    if(done) return;
  
    if(rho < 0.001) // close enough
    {
      System.out.println("dx="+dx+", dy="+dy+", theta="+theta);
      done = true;
    }
    else if(rho < 0.005) {dt = 0.0005; tstep = 0.0;}
    else if(rho < 0.01) {dt = 0.001; tstep = 0.001;}
    else if(rho < 0.1) {dt = 0.01;  tstep = 0.01;}

    // compute deltas from control system
    drho = -kr*rho*Math.cos(alpha);
    dalpha = kr*Math.sin(alpha) - ka*alpha - kb*beta;
    dbeta  = -kr*Math.sin(alpha);
    dtheta = -(dbeta+dalpha);
    
    // robot has to move distance drho*dt
    //       at angle theta+dtheta*dt
    dx = dx + drho*dt*Math.cos(theta+dtheta*dt);
    dy = dy + drho*dt*Math.sin(theta+dtheta*dt);
    rho = Math.sqrt(dx*dx+dy*dy);
    theta = theta+dtheta*dt;
    alpha = -(theta - Math.atan2(dy, dx));
    beta  = -(theta + alpha);
    t = t+dt; // simulation time 
    repaint();
  } // end physicsComputation
  
  public void paint(Graphics g)
  {
    double offsetx, offsety, sinth, costh;
    
    g.setColor(Color.red); // goal
    g.drawLine(p(-10.0), p(  0.0), p(10.0), p( 0.0));
    g.drawLine(p(  0.0), p(-10.0), p( 0.0), p(10.0));

    //glTranslatef(-dx*scale, -dy*scale, 0.0);
    //glRotatef(theta*180.0/Math.PI, 0.0, 0.0, 1.0);
    // use, p, px, py
    
    g.setColor(Color.black); // robot
    //glBegin(GL_LINE_LOOP);   // box
    //  glVertex2f( -5.0, -10.0);
    //  glVertex2f(+30.0, -10.0);
    //  glVertex2f(+30.0, +10.0);
    //  glVertex2f( -5.0, +10.0);
    //glEnd();
    g.drawLine(p(px( -5.0,-10.0)), q(py( -5.0,-10.0)),
               p(px(+30.0,-10.0)), q(py(+30.0,-10.0)));
    g.drawLine(p(px(+30.0,-10.0)), q(py(+30.0,-10.0)),
               p(px(+30.0,+10.0)), q(py(+30.0,+10.0)));
    g.drawLine(p(px(+30.0,+10.0)), q(py(+30.0,+10.0)),
               p(px( -5.0,+10.0)), q(py( -5.0,+10.0)));
    g.drawLine(p(px( -5.0,+10.0)), q(py( -5.0,+10.0)),
               p(px( -5.0,-10.0)), q(py( -5.0,-10.0)));
    //glBegin(GL_LINE_LOOP);   // wheel
    //  glVertex2f(-10.0, -10.0);
    //  glVertex2f(+10.0, -10.0);
    //  glVertex2f(+10.0, -14.0);
    //  glVertex2f(-10.0, -14.0);
    //glEnd();
    g.drawLine(p(px(-10.0,-10.0)), q(py(-10.0,-10.0)),
               p(px(+10.0,-10.0)), q(py(+10.0,-10.0)));
    g.drawLine(p(px(+10.0,-10.0)), q(py(+10.0,-10.0)),
               p(px(+10.0,-14.0)), q(py(+10.0,-14.0)));
    g.drawLine(p(px(+10.0,-14.0)), q(py(+10.0,-14.0)),
               p(px(-10.0,-14.0)), q(py(-10.0,-14.0)));
    g.drawLine(p(px(-10.0,-14.0)), q(py(-10.0,-14.0)),
               p(px(-10.0,-10.0)), q(py(-10.0,-10.0)));
    //glBegin(GL_LINE_LOOP);   // wheel
    //  glVertex2f(-10.0, +10.0);
    //  glVertex2f(+10.0, +10.0);
    //  glVertex2f(+10.0, +14.0);
    //  glVertex2f(-10.0, +14.0);
    //glEnd();
    g.drawLine(p(px(-10.0,+10.0)), q(py(-10.0,+10.0)),
               p(px(+10.0,+10.0)), q(py(+10.0,+10.0)));
    g.drawLine(p(px(+10.0,+10.0)), q(py(+10.0,+10.0)),
               p(px(+10.0,+14.0)), q(py(+10.0,+14.0)));
    g.drawLine(p(px(+10.0,+14.0)), q(py(+10.0,+14.0)),
               p(px(-10.0,+14.0)), q(py(-10.0,+14.0)));
    g.drawLine(p(px(-10.0,+14.0)), q(py(-10.0,+14.0)),
               p(px(-10.0,+10.0)), q(py(-10.0,+10.0)));

    g.setColor(Color.green); // cg
    //glBegin(GL_LINES); 
    //  glVertex2f(-10.0, 0.0);
    //  glVertex2f(+10.0, 0.0);
    //  glVertex2f(0.0, -10.0);
    //  glVertex2f(0.0, +10.0);
    //glEnd();
    g.setColor(Color.green); // goal
    g.drawLine(p(px(-10.0,  0.0)), q(py(-10.0,  0.0)),
               p(px( 10.0,  0.0)), q(py( 10.0,  0.0)));
    g.drawLine(p(px(  0.0,-10.0)), q(py(  0.0,-10.0)),
               p(px(  0.0, 10.0)), q(py(  0.0, 10.0)));
  } // end paint

  public static void main(String args[])
  {
    new Kine();
  }
} // end main
// end Kine.java