/* kine2r.c  control system from CH 3 S&N Autonomous Mobile Robots */
/* given starting position, goal is origin, control vector */
/* navagate to goal. From any x1,y1,theta1 to x2,y2,theta2 translate x2,y2 */
/* the origin. Rotate entire frame of reference for theta2 at zero */
/* Use a linear control system to compute motion derivitives. */
/* Limit motion to mechanical constraints. Compute servo commands */

/* plot movement from x1,y1,theta1 to x2,y2,theta2 */

#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; /* kb was -1.5; */
/* chosen starting point */
static double xi1 = -0.4, yi1 = -0.5, theta1 = -22.0; /* from  */
static double xi2 =  0.5, yi2 =  0.5, theta2 = 15.0; /* to */
/* robot parameters */
static double rwheel = 0.01;  /* radius of wheel, dia/2 */
static double wwidth = 0.025; /* width of wheel centers, b */
static double maxdps = 350.0; /* max degrees per second */
static int maxcommand = 250;  /* command for maximum speed */
static int stopcommand = 128; /* command for zero speed */
                              /* implication, max reverse 128-(250-128)=6 */
                              
static double t = 0.0, gdt = 0.01; /* simulated time and global delta time */
static double dt; /* 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 delx, dely; /* delta x and delta y to move dx,dy in this dt*/
static double alpha, beta, theta, dalpha, dbeta, dtheta;
              /* angles and delta angles in control system coordinates */
static double rho, drho; /* distance to goal, delta distance computed */
static double rwheels, lwheels, drang, dlang; /* wheel speed and delta angle */
static double theta1r, theta2r;

/* graphics controls */
static int main_window;
static int wheel_window;
static int trace = 0;
static int done = 0;
static int cycle = 0;
static float xt[2000], yt[2000], tt[2000];
static int nt = 0;
static float wt[2000], wr[2000], wl[2000];
static int nw = 0;

static void init(void)
{
  printf("kine.c starting \n");
  dx = xi1-xi2; /* delta position to origin */
  dy = yi1-yi2;
  rho = sqrt(dx*dx+dy*dy);
  /* change theta1 and theta2 to radians */
  theta1r = theta1*Pi/180.0;
  theta2r = theta2*Pi/180.0;
  
  theta = theta1r - theta2r;
  rho   = sqrt(dx*dx+dy*dy);
  beta  = atan2(dy, dx);  /* angle robot cg to goal */
  alpha = (-beta)-theta;  /* angle robot X to goal */
  dx    = -dx; /* negate for control system equations */
  dy    = -dy;
  
  drho = 0.0;
  dalpha = 0.0;
  dbeta = 0.0;
  dt = gdt;
  t = 0.0;
  tlast = 0.0;
  done = 0;
  nt = 0;
  nw = 0;
  cycle = 2;
} /* 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) return;
  if(rho < 0.002) /* close enough */
  {
    printf("t=%f, dx=%f, dy=%f, theta=%f \n", t, dx, dy, theta);
    done = 1;;
  }
  dt = gdt;
  atstep = tstep;

/*  if(rho < 0.005)     {dt = gdt/10.0; atstep = tstep/10.0;}
  else if(rho < 0.01) {dt = gdt/5.0;  atstep = tstep/5.0;}
  else if(rho < 0.1)  {dt = gdt/2.0;  atstep = tstep/2.0;}
  if(cycle<50 && rho>0.1) dt=0.005;
*/

  /* 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;
    
  /* control wants to move distance drho*dt */
  /*       at angle theta+dtheta*dt         */
  delx = drho*dt*cos(theta+dtheta*dt);
  dely = drho*dt*sin(theta+dtheta*dt);
  
  /* check if physical capability is exceeded         */
  /* effectively limit movement by using a smaller dt */
  
  
  dx = dx + delx;
  dy = dy + dely;
  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;
  if(cycle==1) wait(2.0);
  cycle++;
  glutSetWindow(main_window);  
  glutPostRedisplay();
  glutSetWindow(wheel_window);
  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 printsmall(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_12, string[i]);
} /* end printsmall */

static void marker(float x, float y, float ang)
{
  glPushMatrix();
    glLoadIdentity();
    glTranslatef(x, y, 0.0);
    glRotatef(ang*180.0/Pi, 0.0, 0.0, 1.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();
  glPopMatrix();
}

static void display(void)
{
  double tprint, scale=200.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(0.0, 0.0, 0.0); /* from */
  glLineWidth(2.0);
  marker(xi1*scale, yi1*scale, theta1r);

  glColor3f(0.3, 0.3, 0.3); /* to */
  glLineWidth(2.0);
  marker(xi2*scale, yi2*scale, theta2r);
  
  glColor3f(1.0, 0.0, 0.0); /* goal */
  glLineWidth(2.0);
  marker(0.0, 0.0, 0.0);
  
  if(trace) /* trace movement of cg */
  {
    xt[nt] = -dx*scale; /* dx and dy opposite normal plot */
    yt[nt] = -dy*scale;
    tt[nt] = t;
    nt++;
    if(nt>1999) nt=1999;
    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(0.0, 0.0, 0.0); /* real */
    glPointSize(1.0);
    glBegin(GL_POINTS); 
      for(i=0; i<nt; i++) glVertex2f(xt[i]+xi2*scale, yt[i]+yi2*scale);
    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);
  marker(-dx*scale, -dy*scale, theta);
/*  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 displayWheels(void)
{
  double  y, tscale = 100.0;
  double angright, angleft, tprint;
  int i;
  double del2S, delthetab, delSr, delSl;
  
  glClear(GL_COLOR_BUFFER_BIT);
  glLoadIdentity();
  glColor3f(0.0, 1.0, 0.0); /* zero velocity */
  printstring( 10.0, 480.0, "right wheel");
  printsmall ( 25.0, 463.0, "fwd   rev");
  printstring(110.0, 480.0, "left wheel");
  printsmall (125.0, 463.0, "fwd   rev");
  printstring(210.0, 480.0, "difference");
  printsmall (220.0, 463.0, "right   left 10x");
  printstring(100.0,  10.0, "linear time down");
  glLineWidth(1.0);
  glBegin(GL_LINES); 
    glVertex2f( 50.0,  30.0);
    glVertex2f( 50.0, 460.0);
    glVertex2f(150.0,  30.0);
    glVertex2f(150.0, 460.0);
    glVertex2f(250.0,  30.0);
    glVertex2f(250.0, 460.0);
  glEnd();

  /* given radius of wheel, rwheel, */
  /* given angle wheel turns, dwang, in radians*/
  /* the linear distance moved is  rwheel*dwang */

  /* given the wheel moves dwang in dt seconds, */
  /* the angular velocity is dwang/dt  radians per second */
  /* the wheel speed is (dwang/dt)*(180/Pi) degrees per second */
  /* the wheel speed is (dwand/dt)/2Pi revolutions per second */
  
  /* given the distance moved in time dt, as drho, */
  /* given the direction angle change dtheta, */
  /* given the distance between the center of the wheels, wwidth */
  /* in time dt, the wheels move linearly */
  
  glColor3f(0.0, 0.0, 1.0); /* wheel velocity */
  y = 455.0 - t * tscale;
  del2S = 2.0*sqrt(delx*delx+dely*dely);
  delthetab = dtheta*dt*wwidth;
  delSr = (delthetab+del2S)/2.0;
  delSl = del2S-delSr;
  angright = -delSr/rwheel;
  angleft  = -delSl/rwheel;
  wr[nw] = ((angright)/dt)*(180.0/Pi); /* speed in degrees per second */
  wl[nw] = (( angleft)/dt)*(180.0/Pi);
  wt[nw] = y;
  printf("\n drho=%f, drho*dt=%f, dalpha=%f, dt=%f \n",
             drho, drho*dt, dalpha, dt);
  printf("delSr=%f, delSl=%f, dtheta=%f , dbeta=%f \n",
          delSr, delSl, dtheta, dbeta);
  printf("angright=%f, angleft=%f \n", angright, angleft);
  printf("wr[nw]=%f, wl[nw]=%f, t=%f \n", wr[nw], wl[nw], t);
  nw++;
  if(nw>1999) nw=1999;
  glBegin(GL_LINES); 
    for(i=1; i<nw-1; i++)
    { 
      glVertex2f( 50.0+wr[i]  /500.0, wt[i]);
      glVertex2f( 50.0+wr[i+1]/500.0, wt[i+1]);
      glVertex2f(150.0+wl[i]  /500.0, wt[i]);
      glVertex2f(150.0+wl[i+1]/500.0, wt[i+1]);
      glVertex2f(250.0+(wl[i]-wr[i])    /50.0, wt[i]);
      glVertex2f(250.0+(wl[i+1]-wr[i+1])/50.0, wt[i+1]);
    }
  glEnd();
  if(trace)
  {
    tprint = 0.0;
    glColor3f(1.0, 0.0, 0.0); /* marker */
    glBegin(GL_LINES); 
      for(i=0; i<nw; i++)
      {
        if(tt[i]>tprint+0.1)
        {
          tprint = tprint + 0.1;
          glVertex2f(  0.0, wt[i]);
          glVertex2f(300.0, wt[i]);
        }
      }
    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);
  glutPostRedisplay();
}

int main(int argc, char *argv[])
{
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
  glutInitWindowSize(500, 500);
  glutInitWindowPosition(100,100); 
  main_window = 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);

  glutInitWindowSize(300, 500);
  glutInitWindowPosition(608,100); 
  wheel_window = glutCreateWindow("wheels");
  glutDisplayFunc(displayWheels);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(0.0, (GLfloat)300, 0.0, (GLfloat)500);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glClearColor(1.0, 1.0, 1.0, 0.0);

  init();
  glutMainLoop();
  return 0;
}