/* kine4.c  display four windows of data, control and report window */
/*          3D control system to reach 0,0,0 from negative X        */

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

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

static int XZw;             /* XZ display window handle */
static int orthow;          /* ortho display window handle */
static int XYw;             /* XY display window handle */
static int YZw;             /* YZ vector display window handle */
static int controlw;        /* control window handle */
static int reportw;         /* report window handle */
static int width   = 300;   /* window width */
static int height  = 300;   /* window height */
static int cwidth  = 300;   /* control width */
static int trace = 0;       /* trace path */
static int run = 0;         /* automatic run */
static float scale = 1.0;   /* window scale */
static int xm;              /* mouse x */
static int ym;              /* mouse y corrected */
static double t = 0.0;      /* simulation time */
static double dt = 0.02;    /* simulation step time */

typedef struct {float x; float y; float z; float ax; float ay; float az;} ptype;
static ptype p[8000]; /* for plotting lication each delta t */
static np = 0;
static float view_theta = 22.0;  /* object rotation about X axis */
static float view_phi   = 16.0;  /* object rotation about Z axis */


/* chosen control parameters */
static double kr = 3.0, ka = 8.0, kb = -3.0; /* was -1.5; */
/* chosen starting point */
static double dx, dy, dz; /* goal is origin */

/* state values in XY plane */
static double alpha, beta, theta, dalpha, dbeta, dtheta;
static double rho, drho;
/* state values in Z-XY plane */
static double alphaz, betaz, thetaz, dalphaz, dbetaz, dthetaz;
static double rhoz, drhoz;
static double dxy; /* length of XY projection */
static double ax, ay, az;
static double delx, dely, delz, delxy, delax, delay, delaz;
static int position = 0;
static double dxp[16] = { 0.7,  0.0, -0.7, -1.0, -0.7,  0.0,  0.7,  1.0,
                          0.7,  0.0, -0.7, -1.0, -0.7,  0.0,  0.7,  1.0};
static double dyp[16] = { 0.7,  1.0,  0.7,  0.0, -0.7, -1.0, -0.7,  0.0,
                          0.6,  0.9,  0.6,  0.1, -0.6, -0.9, -0.6, -0.1};
static double dzp[16] = { 0.7,  0.7,  0.7,  0.7,  0.7,  0.7,  0.7,  0.7,
                         -0.7, -0.7, -0.7, -0.7, -0.7, -0.7, -0.7, -0.7};

/* function prototypes */
static void init(void);
static void physics(void);
static void draw_box(float x1, float x2, float y1, float y2, float z1, float z2);
static writeText(GLfloat x, GLfloat y, GLfloat z, GLfloat size, char * msg);
static void display_XZ(void);
static void display_ortho(void);
static void display_XY(void);
static void display_YZ(void);
static void display_control(void);
static void update_displays(void);   /* all */
static void myinit(void);
static void mouse(int btn, int state, int x, int y);
static void mouse_control(int btn, int state, int x, int y);
static int in_rect(int x, int y, int w, int h);
static void keys(unsigned char key, int x, int y);
static void reshape(int w, int h);
static void runFunc(void);
static void controlReshape(int w, int h);
    /* int  main(int argc, char * argv[]); */
static void finish(void);

static void init(void)
{
  printf("kine4.c starting position %d\n", position);
  t = 0.0;
  dx = dxp[position]; /* delta position to origin */
  dy = dyp[position];
  dz = dzp[position];
  alpha = Pi/8.0;  /* velocity angle from robot X in Y direction */
  theta = atan2(dy, dx); /* -(beta+alpha)  angle robot X to inertial */
  beta  = -(theta+alpha); /* angle robot cg to goal */
  rho = sqrt(dx*dx+dy*dy);
  dalpha = 0.0;
  dbeta = 0.0;
  dtheta = 0.0;
  drho = 0.0;
  dxy = rho;
  alphaz = Pi/10.0; /* velocity angle from robot X in Z direction */
  thetaz = atan2(dz, dxy); /* in spherical, phi = Pi - thetaz */ 
  betaz = -(thetaz+alphaz);
  rhoz = sqrt(dxy*dxy+dz*dz); /* = sqrt(dx*dx+dy*dy+dz*dz) */
  dalphaz = 0.0;
  dbetaz = 0.0;
  dthetaz = 0.0;
  drhoz = 0.0;
  ax = 0.1*cos(theta+alpha);
  ay = 0.1*sin(theta+alpha);
  az = 0.1*cos(thetaz+alphaz);
  p[np].x = dx;;
  p[np].y = dy;
  p[np].z = dz;
  p[np].ax = ax;
  p[np].ay = ay;
  p[np].az = az;
  np++;
} /* end init */

static void physics(void)
{

  /* 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);
    
  drhoz = -kr*rhoz*cos(alphaz);
  dalphaz = kr*sin(alphaz) - ka*alphaz - kb*betaz;
  dbetaz  = -kr*sin(alphaz);
  dthetaz = -(dbetaz+dalphaz);
    
  /* robot has to move distance drho*dt */
  /*       at angle theta+dtheta*dt     */
  delx = drho*dt*cos(theta+dtheta*dt);
  dx = dx + delx;
  dely = drho*dt*sin(theta+dtheta*dt);
  dy = dy + dely;
  rho = sqrt(dx*dx+dy*dy);
  theta = theta+dtheta*dt;
  alpha = -(theta - atan2(dy, dx));
  beta  = -(theta + alpha);

  delxy = drhoz*dt*cos(thetaz+dthetaz*dt);
  dxy = dxy + delxy;
  delz = drhoz*dt*sin(thetaz+dthetaz*dt);
  dz = dz + delz;
  rhoz = sqrt(dxy*dxy+dz*dz);
  thetaz = thetaz+dthetaz*dt;
  alphaz = -(thetaz - atan2(dz, dxy));
  betaz  = -(thetaz + alphaz);

  ax = 0.1*cos(theta+alpha);
  ay = 0.1*sin(theta+alpha);
  az = 0.1*cos(thetaz+alphaz);
  printf("delx=%f, dely=%f, delz=%f\n\n",
         delx, dely, delz);
  p[np].x = dx;;
  p[np].y = dy;
  p[np].z = dz;
  p[np].ax = ax;
  p[np].ay = ay;
  p[np].az = az;
  np++;
  if(np>7999) np--;
  
  t = t + dt;
  if(abs(dx)+abs(dy)+abs(dz)<0.01)
  {
    position++;
    if(position>15)
    {
      position = 0;
      run = 0;
    }
    init();
  }
}

static writeText(GLfloat x, GLfloat y, GLfloat z, GLfloat size, char * msg)
{
  char * p;
  
  glPushMatrix();
    glLoadIdentity ();
    glEnable(GL_LINE_SMOOTH);
    glEnable(GL_BLEND);
    glTranslatef(x, y, z);
    glScalef(size/1000.0, size/1000.0, 0.0);
    for(p=msg; *p; p++)
      glutStrokeCharacter(GLUT_STROKE_MONO_ROMAN, *p);
  glPopMatrix();
} /* end writeText, may have to reset glEnable's after call */

static void display_XZ(void)
{
  int i;
  
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glLoadIdentity();
  glColor3f (0.0, 0.0, 0.0);
  glPointSize(2.0);
  glBegin(GL_POINTS); 
    for(i=0; i<np; i++) glVertex2f(-p[i].x*scale, p[i].z*scale);
  glEnd();
  glColor3f (1.0, 0.0, 0.0);
  glBegin(GL_LINES); 
    glVertex2f(0.0, 0.0);
    glVertex2f(0.08, 0.0);
    glVertex2f(0.05, 0.02);
    glVertex2f(0.08, 0.0);
    glVertex2f(0.05, -0.02);
    glVertex2f(0.08, 0.0);
  glEnd();
  glFlush();
} /* end display_XZ */

static void display_ortho(void)
{
  int i;
  double theta_deg, thetaz_deg, y_deg;
  
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glLoadIdentity();
  glRotatef(view_theta, 1.0, 0.0, 0.0);
  glRotatef(view_phi,   0.0, 0.0, 1.0);
  glColor3f (0.0, 0.0, 0.0);
  glPointSize(2.0);
  glBegin(GL_POINTS); 
    for(i=0; i<np; i++) glVertex3f(-p[i].x*scale, -p[i].y*scale, p[i].z*scale);
  glEnd();
  glColor3f (0.0, 0.0, 1.0);
  glLineWidth(2.0);
  glPushMatrix();
    glTranslatef(-p[np-1].x*scale,
                 -p[np-1].y*scale,
                  p[np-1].z*scale);
    theta_deg = theta*180.0/Pi;
    thetaz_deg = thetaz*180.0/Pi;
    y_deg = 90.0;
    glRotatef(theta_deg,  0.0, 0.0, 1.0);
    glRotatef(y_deg,      0.0, 1.0, 0.0);
    glRotatef(thetaz_deg, 0.0, 1.0, 0.0);
    glutSolidCone(0.05, 0.2, 6, 6);
  glPopMatrix();
  glColor3f (1.0, 0.0, 0.0);
  glBegin(GL_LINES); 
    glVertex3f(-1.3, 0.0, 0.0);
    glVertex3f( 1.3, 0.0, 0.0);
    glVertex3f(0.0, -1.3, 0.0);
    glVertex3f(0.0,  1.3, 0.0);
    glVertex3f(0.0, 0.0, -1.3);
    glVertex3f(0.0, 0.0,  1.3);
  glEnd();
  glPushMatrix();
    glTranslatef(1.3, 0.0, 0.0);
    glRotatef(90.0, 0.0, 1.0, 0.0); /* point +X */
    glutSolidCone(0.05, 0.2, 6, 6);
  glPopMatrix();
  glPushMatrix();
    glColor3f (1.0, 1.0, 0.0);
    glTranslatef(0.0, 1.3, 0.0);
    glRotatef(90.0, 1.0, 0.0, 0.0); /* point +Y */
    glRotatef(180.0, 0.0, 1.0, 0.0); /* point +Y */
    /* glutSolidCone(0.05, 0.2, 6, 6); */
  glPopMatrix();
  glPushMatrix();
    glColor3f (0.0, 1.0, 1.0);
    glTranslatef(0.0, 0.0, 1.3);
    glRotatef(00.0, 1.0, 0.0, 0.0); /* point +Z */
    glRotatef(90.0, 0.0, 0.0, 1.0); /* point +Z */
    /* glutSolidCone(0.05, 0.2, 6, 6); */
  glPopMatrix();
  glFlush();
} /* end display_ortho */

static void display_XY(void)
{
  int i;
  
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glLoadIdentity();
  glColor3f (0.0, 0.0, 0.0);
  glPointSize(2.0);
  glBegin(GL_POINTS); 
    for(i=0; i<np; i++) glVertex2f(-p[i].x*scale, -p[i].y*scale);
  glEnd();
  glColor3f (1.0, 0.0, 0.0);
  glBegin(GL_LINES); 
    glVertex2f(0.0, 0.0);
    glVertex2f(0.08, 0.0);
    glVertex2f(0.05, 0.02);
    glVertex2f(0.08, 0.0);
    glVertex2f(0.05, -0.02);
    glVertex2f(0.08, 0.0);
  glEnd();
  glFlush();
} /* end display_XY */

static void display_YZ(void)
{
  int i;
  
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glLoadIdentity();
  glColor3f (0.0, 0.0, 0.0);
  glPointSize(2.0);
  glBegin(GL_POINTS); 
    for(i=0; i<np; i++) glVertex2f(-p[i].y*scale, p[i].z*scale);
  glEnd();
  glColor3f (1.0, 0.0, 0.0);
  glBegin(GL_POINTS); 
    glVertex2f(0.0, 0.0); /* only point */
  glEnd();
  glFlush();
} /* end display_YZ */

static void display_control(void)
{
  float fh;
  
  fh = (float)height;
  glClear(GL_COLOR_BUFFER_BIT);
  glLoadIdentity();
  glColor3f(0.0, 0.0, 0.0);
  writeText(5.0, fh- 25.0, 0.0, 100.0, "key commands");
  writeText(5.0, fh- 45.0, 0.0, 100.0, "x,X,y,Y,z,Z rotate");
  writeText(5.0, fh- 65.0, 0.0, 100.0, "d  scale down");
  writeText(5.0, fh- 85.0, 0.0, 100.0, "u  scale up");
  writeText(5.0, fh-105.0, 0.0, 100.0, "s  step");

  glColor3f(0.0, 1.0, 1.0);
  glBegin(GL_POLYGON);
    glVertex3f(0.0,1.0,0.0);
    glVertex3f((float)cwidth,1.0,0.0);
    glVertex3f((float)cwidth,21.0,0.0);
    glVertex3f(0.0,21.0,0.0);
  glEnd();
  glColor3f(0.0, 0.0, 0.0);
  writeText(5.0, 4.0, 0.0, 150.0, "clear");

  glColor3f(1.0, 1.0, 0.0);
  glBegin(GL_POLYGON);
    glVertex3f(0.0,23.0,0.0);
    glVertex3f((float)cwidth,23.0,0.0);
    glVertex3f((float)cwidth,43.0,0.0);
    glVertex3f(0.0,43.0,0.0);
  glEnd();
  glColor3f(0.0, 0.0, 0.0);
  writeText(5.0, 26.0, 0.0,150.0, "exit");

  glColor3f(0.0, 1.0, 0.0);
  glBegin(GL_POLYGON);
    glVertex3f(0.0,45.0,0.0);
    glVertex3f((float)cwidth,45.0,0.0);
    glVertex3f((float)cwidth,65.0,0.0);
    glVertex3f(0.0,65.0,0.0);
  glEnd();
  glColor3f(0.0, 0.0, 0.0);
  writeText(5.0, 48.0, 0.0, 150.0, "run");

  glFlush();
} /* end display_control */

static void display_report(void)
{
  char t_val[]     = "T  =         seconds";
  char dt_val[]    = "dT =         seconds";
  char dx_val[]    = "dx =          dist";
  char dy_val[]    = "dy =          dist";
  char dz_val[]    = "dz =          dist";
  char delx_val[]  = "delx =         dist";
  char dely_val[]  = "dely =         dist";
  char delz_val[]  = "delz =         dist";
  char ax_val[]    = "ax =           radians";
  char ay_val[]    = "ay =           radians";
  char az_val[]    = "az =           radians";
  char alpha_val[] = "alpha =        radians";
  char beta_val[]  = "beta =        radians";
  char theta_val[] = "theta =        radians";
  
  glClear(GL_COLOR_BUFFER_BIT);
  glLoadIdentity();
  glColor3f(0.0, 0.0, 0.0);
  sprintf(&t_val[5], "%7.3f", t);
  t_val[12]=' '; /* remove null from sprintf string */
  writeText(5.0, (float)(height-30), 0.0, 100.0, t_val);

  sprintf(&dt_val[5], "%7.3f", dt);
  dt_val[12]=' ';
  writeText(5.0, (float)(height-50), 0.0, 100.0, dt_val);

  sprintf(&dx_val[8], "%5.3f", dx);
  dx_val[13]=' ';
  writeText(5.0, 45.0, 0.0, 100.0, dx_val);
  sprintf(&dy_val[8], "%5.3f", dy);
  dy_val[13]=' ';
  writeText(5.0, 25.0, 0.0, 100.0, dy_val);
  sprintf(&dz_val[8], "%5.3f", dz);
  dz_val[13]=' ';
  writeText(5.0,  5.0, 0.0, 100.0, dz_val);

  sprintf(&delx_val[8], "%5.3f", delx);
  delx_val[13]=' ';
  delx_val[14]=' ';
  writeText(5.0,105.0, 0.0, 100.0, delx_val);
  sprintf(&dely_val[8], "%5.3f", dely);
  dely_val[13]=' ';
  dely_val[14]=' ';
  writeText(5.0, 85.0, 0.0, 100.0, dely_val);
  sprintf(&delz_val[8], "%5.3f", delz);
  delz_val[13]=' ';
  delz_val[14]=' ';
  writeText(5.0, 65.0, 0.0, 100.0, delz_val);

  sprintf(&ax_val[8], "%5.3f", ax);
  ax_val[13]=' ';
  ax_val[14]=' ';
  writeText(5.0,165.0, 0.0, 100.0, ax_val);
  sprintf(&ay_val[8], "%5.3f", ay);
  ay_val[13]=' ';
  ay_val[14]=' ';
  writeText(5.0,145.0, 0.0, 100.0, ay_val);
  sprintf(&az_val[8], "%5.3f", az);
  az_val[13]=' ';
  az_val[14]=' ';
  writeText(5.0,125.0, 0.0, 100.0, az_val);
  
  sprintf(&alpha_val[8], "%5.3f", alpha);
  alpha_val[13]=' ';
  alpha_val[14]=' ';
  writeText(5.0,225.0, 0.0, 100.0, alpha_val);
  sprintf(&beta_val[8], "%5.3f", beta);
  beta_val[13]=' ';
  beta_val[14]=' ';
  writeText(5.0,205.0, 0.0, 100.0, beta_val);
  sprintf(&theta_val[8], "%5.3f", theta);
  theta_val[13]=' ';
  theta_val[14]=' ';
  writeText(5.0,185.0, 0.0, 100.0, theta_val);
  glFlush();
} /* end display_report */

static void update_displays(void)   /* all */
{
  glutSetWindow(XZw);   /* draw XZ */
  glutPostRedisplay(); 
  glutSetWindow(orthow);  /* draw ortho */
  glutPostRedisplay();
  glutSetWindow(XYw);  /* draw XY */
  glutPostRedisplay(); 
  glutSetWindow(YZw);     /* draw YZ */
  glutPostRedisplay();
  glutSetWindow(controlw);   /* draw control */
  glutPostRedisplay(); 
  glutSetWindow(reportw);   /* draw report */
  glutPostRedisplay(); 
} /* end update_displays */

static void myinit(void) /* shared */
{
  glClearColor (1.0, 1.0, 1.0, 1.0);
  glColor3f (0.0, 0.0, 0.0);
} /* end myinit */

static void mouse(int btn, int state, int x, int y) /* shared */
{
  if(btn==GLUT_LEFT_BUTTON && state==GLUT_DOWN) 
  if(btn==GLUT_RIGHT_BUTTON && state==GLUT_DOWN)
  update_displays();
} /* end mouse */

static void mouse_control(int btn, int state, int x, int y)
{
  xm = x; /* global mouse coordinates */
  ym = height-y;
  if(btn==GLUT_LEFT_BUTTON && state==GLUT_DOWN)
  {
    if(in_rect(0, 1, cwidth, 20)) /* clear */
    {
      np = 0;
      init();
      update_displays();
    }
    else if(in_rect(0, 23, cwidth, 20)) /* exit */
    {
      finish();
    }
    else if(in_rect(0, 45, cwidth, 20)) /* run */
    {
      run = 1 - run;
      update_displays();
    }
  }
} /* end mouse_control */

static int in_rect(int x, int y, int w, int h)
{
  return xm>x && xm<x+w && ym>y && ym<y+h;
} /* end in_rect */

static void keys(unsigned char key, int x, int y)
{
  /* Use x, X, y, Y, z, Z */
  if(key == 'x') view_theta -= 2.0;
  if(key == 'X') view_theta += 2.0;
  if(key == 'z') view_phi   -= 2.0;
  if(key == 'Z') view_phi   += 2.0;
  if(key == 's') physics();
  if(key == 'c') trace = 1 - trace;
  if(key == 'u') scale = scale * 1.414;
  if(key == 'd') scale = scale / 1.414;
  if(key == 'q') finish(); /* quit */
  update_displays();
} /* end keys */

static void reshape(int w, int h) /* shared XY, XZ, YZ*/
{
  width = w;
  height = h;
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(-1.0, 1.0, -1.0, 1.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity ();
  glEnable(GL_DEPTH_TEST);
} /* end reshape */

static void controlReshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(0.0, (float)cwidth, 0.0, (float)height); /* all at z=0 */
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity ();
} /* end controlReshape */

static void runFunc(void)
{
  if(run)
  {
    physics();
    update_displays();
  }
}

static void orthoReshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glOrtho(-1.4, 1.4, -1.4, 1.4, -1.4, 1.4);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity ();
} /* end controlReshape */

int main(int argc, char* argv[])
{
  glutInit(&argc,argv);
  glutInitDisplayMode (GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH);

  glutInitWindowPosition(25,50);
  glutInitWindowSize(cwidth,height);
  controlw=glutCreateWindow("kine4");
  myinit();
  glutDisplayFunc(display_control);
  glutReshapeFunc(controlReshape);
  glutMouseFunc(mouse_control);
  glutKeyboardFunc(keys);

  glutInitWindowPosition(25,50+height+34);
  glutInitWindowSize(cwidth,height);
  reportw=glutCreateWindow("report");
  myinit();
  glutDisplayFunc(display_report);
  glutReshapeFunc(controlReshape);
  glutMouseFunc(mouse);
  glutKeyboardFunc(keys);

  glutInitWindowPosition(25+cwidth+8,50);
  glutInitWindowSize(width,height);
  XZw=glutCreateWindow("XZ");
  myinit();
  glutDisplayFunc(display_XZ); 
  glutReshapeFunc(reshape);
  glutMouseFunc(mouse);
  glutKeyboardFunc(keys);

  glutInitWindowPosition(25+cwidth+8+width+8,50);
  glutInitWindowSize(width,height);
  orthow=glutCreateWindow("ortho");
  myinit();
  glutDisplayFunc(display_ortho);
  glutReshapeFunc(orthoReshape);
  glutMouseFunc(mouse);
  glutKeyboardFunc(keys);

  glutInitWindowPosition(25+cwidth+8,50+height+34);
  glutInitWindowSize(width,height);
  XYw=glutCreateWindow("XY");
  myinit();
  glutDisplayFunc(display_XY); 
  glutReshapeFunc(reshape);
  glutMouseFunc(mouse);
  glutKeyboardFunc(keys);

  glutInitWindowPosition(25+cwidth+8+width+8,50+height+34);
  glutInitWindowSize(width,height);
  YZw=glutCreateWindow("YZ");
  myinit();
  glutDisplayFunc(display_YZ);
  glutReshapeFunc(reshape);
  glutMouseFunc(mouse);
  glutKeyboardFunc(keys);
  glutIdleFunc(runFunc);

  init();
  glutMainLoop();
  return 0; /* exit should be via "quit" to finish() */
} /* end main */

static void finish(void)
{
  exit(0);
} /* end finish */