/* kine3.c  from CH 3 S&N Autonomous Mobile Robots */
/* given starting position, goal is origin, control vector */

#include <stdio.h>
#include <stdlib.h>
#include <string.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))
/* menu item numbers */
#define RESTART   1
#define SLOWER    2
#define FASTER    3
#define TRACE     4
#define QUIT      5

/* chosen control parameters */
static double kr = 3.0, ka = 8.0, kb = -3.0; /* was -1.5; */
/* computed starting point */
static double start_ang = 0.0;

static double t = 0.0, gdt = 0.01; /* simulated time and global delta time */
static double dt; /* adjusted for integration, delta time step */
static double tstep = 0.01, tlast = 0.0; /* try 100 step per sec */
static double dx, dy; /* delta x and delta y to goal */
static double alpha, beta, theta, dalpha, dbeta, dtheta;
              /* angles and delta angles */
static double rho, drho; /* distance to goal, delta distance computed */
static double rwheel, lwheel, drang, dlang; /* wheel speed and delta angle */

/* graphics controls */
static int trace = 1;
static int done = 0;
static float xt[4000], yt[4000], tt[4000];
static int nt = 0;

static void init(void)
{
  printf("kine.c starting \n");
  start_ang = 0.0;
  dx = cos(start_ang);
  dy = sin(start_ang);
  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 */
  drho = 0.0;
  dalpha = 0.0;
  dbeta = 0.0;
  dt = gdt;
  t = 0.0;
  tlast = 0.0;
  done = 0;
  nt = 0;
} /* 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 */
  double atstep;
  
  if(done)
  {
    if(start_ang+Pi/6.0+0.1>2.0*Pi) return;
    start_ang = start_ang + Pi/6.0;
    dx = cos(start_ang);
    dy = sin(start_ang);
    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 */
    drho = 0.0;
    dalpha = 0.0;
    dbeta = 0.0;
    dt = gdt;
    tlast = 0.0;
    done = 0;
  }
  if(rho < 0.002) /* close enough */
  {
    printf("t=%f, dx=%f, dy=%f, theta=%f \n", t, dx, dy, theta);
    done = 1;
    return;
  }
  dt = gdt;
  atstep = tstep;
  
  /* 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);

  /* clip and clamp */
  if(abs(drho)>1.0) drho = drho/abs(drho);
  if(abs(drho)<0.1) drho = 0.1*drho/abs(drho);
  if(abs(dtheta)>10.0) dtheta = 10.0*dtheta/abs(dtheta);
  if(abs(dtheta)<1.0) dtheta = dtheta/abs(dtheta);
    
  /* 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+atstep)
  {
    tnow = (double)clock()/(double)CLOCKS_PER_SEC;
  }
  tlast = tnow;
  glutPostRedisplay();
} /* end physicsComputation */


static void printstring(float x, float y, char *string)
{
   int len, i;

   glRasterPos2f(x, y);
   len = (int) strlen(string);
   for (i = 0; i < len; i++)
      glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, string[i]);
} /* end printstring */

static void display(void)
{
  double tprint, scale=180.0;
  int i;
  
  glClear(GL_COLOR_BUFFER_BIT);
  glLoadIdentity();
  glColor3f(0.0, 0.0, 1.0);
  printstring(-240.0, -240.0, "left click for memu and selection");
  if(done) 
  {
    glColor3f(1.0, 0.0, 0.0);
    printstring(40.0, -240.0, "Goal reached");
  }
  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);
    glVertex2f( 10.0,   0.0);
    glVertex2f(  7.0,   3.0);
    glVertex2f( 10.0,   0.0);
    glVertex2f(  7.0,  -3.0);
  glEnd();

  if(trace) /* trace movement of cg */
  {
    xt[nt] = -dx*scale;
    yt[nt] = -dy*scale;
    tt[nt] = t;
    nt++;
    if(nt>3999) nt=3999;
    glColor3f(0.0, 1.0, 0.0); /* goal */
    glPointSize(1.0);
    glBegin(GL_POINTS); 
      for(i=0; i<nt; i++) glVertex2f(xt[i], yt[i]);
    glEnd();
    glColor3f(1.0, 0.0, 0.0); /* marker */
    glPointSize(3.0);
    tprint = 0.0;
    glBegin(GL_POINTS);
      for(i=0; i<nt; i++)
      {
        if(tt[i]>tprint+0.1)
        {
          tprint = tprint + 0.1;
          glVertex2f(xt[i], yt[i]);
        }
      }
    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);
    glVertex2f( 10.0,   0.0);
    glVertex2f(  7.0,   3.0);
    glVertex2f( 10.0,   0.0);
    glVertex2f(  7.0,  -3.0);
  glEnd();
  
  glutSwapBuffers();
}

static void menu_item(int item)
{
  switch(item)
  {
    case RESTART:
      init();
      break;
    case SLOWER:
      tstep = tstep/2.0; /* attempt smooth motion */
      gdt = gdt/2.0;
      break;
    case FASTER:
      tstep = tstep*2.0;
      gdt = gdt*2.0;
      break;
    case TRACE:
      trace = 1-trace;
      break;
    case QUIT:
      exit(0);
      break;
    default:
      break;
  }
  physicsComputation();
} /* end menu_item */

static void reshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  /* fixed size coordinate system */
  gluOrtho2D(-250.0, 250.0, -250.0, 250.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glClearColor(1.0, 1.0, 1.0, 0.0);
}

int main(int argc, char *argv[])
{
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
  glutInitWindowSize(500, 500);
  glutInitWindowPosition(100,100); 
  glutCreateWindow(argv[0]);
  glutCreateMenu(menu_item);
    glutAddMenuEntry("Restart", RESTART);
    glutAddMenuEntry("Slower", SLOWER);
    glutAddMenuEntry("Faster", FASTER);
    glutAddMenuEntry("Toggle trace", TRACE);
    glutAddMenuEntry("Quit", QUIT);
  glutAttachMenu(GLUT_LEFT_BUTTON);
  glutReshapeFunc(reshape);
  glutDisplayFunc(display);
  glutIdleFunc(physicsComputation);
  init();
  glutMainLoop();
  return 0;
}