/* kine.c  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 */

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <GL/glut.h>

#undef  Pi
#define Pi     3.14159265358979323846
#undef  abs
#define abs(x) ((x)<0.0?(-(x)):(x))

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

static void init(void)
{
  printf("kine.c starting \n");
  dx = xi1-xi0; /* delta position to origin */
  dy = yi1-yi0;
  rho = sqrt(dx*dx+dy*dy);
  alpha = Pi/8.0;  /* angle robot X to goal */
  beta  = -Pi/4.0; /* angle robot cg to goal */
  theta = -(beta+alpha);  /* angle robot X to inertial */
} /* end init */

static void wait(double sec)
{
  double now, next;
  now = (double)time(NULL);
  next = now+sec;
  while(now <= next) 
  {
    now = (double)time(NULL);    
  }    
}

static void physicsComputation(void)
{
  double tnow; /* for drawing timing control */
  static int done=0;
  
  if(done) return;
  
  if(rho < 0.001) /* close enough */
  {
    printf("dx=%f, dy=%f, theta=%f \n", dx, dy, theta);
    done = 1;;
  }
  else if(rho < 0.005) {dt = 0.0001; 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*cos(alpha);
  dalpha = kr*sin(alpha) - ka*alpha - kb*beta;
  dbeta  = -kr*sin(alpha);
  dtheta = -(dbeta+dalpha);
    
  /* robot has to move distance drho*dt */
  /*       at angle theta+dtheta*dt     */
  dx = dx + drho*dt*cos(theta+dtheta*dt);
  dy = dy + drho*dt*sin(theta+dtheta*dt);
  rho = sqrt(dx*dx+dy*dy);
  theta = theta+dtheta*dt;
  alpha = -(theta - atan2(dy, dx));
  beta  = -(theta + alpha);
  t = t+dt; /* simulation time */
  tnow = (double)clock()/(double)CLOCKS_PER_SEC;
  while(tnow<tlast+tstep)
  {
    tnow = (double)clock()/(double)CLOCKS_PER_SEC;
  }
  tlast = tnow;
  if(cycle==1) wait(3.0);
  cycle++;
  glutPostRedisplay();
} /* end physicsComputation */

static void reshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(-250.0, 250.0, -250.0, 250.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glClearColor(1.0, 1.0, 1.0, 0.0);
  glutPostRedisplay();
}

static void display(void)
{
  double scale=180.0;
  
  glClear(GL_COLOR_BUFFER_BIT);
  glLoadIdentity();
  glColor3f(1.0, 0.0, 0.0); /* goal */
  glLineWidth(2.0);
  glBegin(GL_LINES); 
    glVertex2f(-10.0, 0.0);
    glVertex2f(+10.0, 0.0);
    glVertex2f(0.0, -10.0);
    glVertex2f(0.0, +10.0);
  glEnd();

  glTranslatef(-dx*scale, -dy*scale, 0.0);
  glRotatef(theta*180.0/Pi, 0.0, 0.0, 1.0);

  glColor3f(0.0, 0.0, 0.0); /* robot */
  glLineWidth(2.0);
  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();
  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();
  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();

  glColor3f(0.0, 0.0, 1.0); /* cg */
  glLineWidth(2.0);
  glBegin(GL_LINES); 
    glVertex2f(-10.0, 0.0);
    glVertex2f(+10.0, 0.0);
    glVertex2f(0.0, -10.0);
    glVertex2f(0.0, +10.0);
  glEnd();
  
  glFlush();
  glutSwapBuffers();
}

int main(int argc, char *argv[])
{
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
  glutInitWindowSize(500, 500);
  glutInitWindowPosition(100,100); 
  glutCreateWindow(argv[0]);
  glutReshapeFunc(reshape);
  glutDisplayFunc(display);
  glutIdleFunc(physicsComputation);
  init();
  glutMainLoop();
  return 0;
}