/* wheel.c  drive a wheel with a standard servo */

#include <GL/glut.h>
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265
#endif
#undef  abs
#define abs(a)  ((a)<0.0?(-a):(a))

static int winWidth = 800;
static int winHeight = 400;
static double x1, y11, a1, r1, da1, px1, py1, l12;
static double x2, y2, a2, r2, da2, px2, py2, rtd;
static double ox1, oy11, oa1, or1, oda1, opx1, opy1, l12;
static double ox2, oy2, oa2, or2, oda2, opx2, opy2;
static double delta = M_PI/720.0;
static int step = 0;
static int debug = 0;

static void init()
{
  double f2;
  
  rtd=180.0/M_PI;
  x1= 600.0;
  y11= 200.0;
  r1=  40.0;
  a1=   0.0;
  px1=x1+r1*cos(a1);
  py1=y11+r1*sin(a1);
  f2= sqrt(1.5*1.5+0.866*0.866);
  r2= r1*f2;
  if(debug) printf("r1=%f, r2=%f, f2=%f \n", r1, r2, f2);
  f2= 1.414;
  r2= r1*f2;
  if(debug) printf("r1=%f, r2=%f, f2=%f \n", r1, r2, f2);
  x2= 200.0;
  y2= 200.0;
  a2= a1+M_PI/6.0; /* arm std */
  a2= a1+0.0;      /* arm rotated */
  px2=x2+r2*cos(a2);
  py2=y2+r2*sin(a2);
  l12=sqrt((px1-px2)*(px1-px2)+(py1-py2)*(py1-py2));

  ox1= x1;
  oy11= 300.0;
  or1=  r1;
  oa1=a1+M_PI/2.0;
  opx1=ox1+or1*cos(a1);
  opy1=oy11+or1*sin(a1);
  ox2= x2;
  oy2= 300.0;
  oa2= oa1;
  or2= r2;
  opx2=ox2+or2*cos(a2);
  opy2=oy2+or2*sin(a2);
} /* end init */

static void min_searcha2()
{
  double tl1, tl2, ta2, tpx2, tpy2, tl3, ta3, tpx3, tpy3;
  double dela = M_PI/60.0;
  int i;
  
  for(i=0; i<12; i++)
  {
    px2=x2+r2*cos(a2);
    py2=y2+r2*sin(a2);
    tl1=sqrt((px1-px2)*(px1-px2)+(py1-py2)*(py1-py2))-l12; /* error */
    tl1=abs(tl1);
    if(tl1<0.05) return;
    ta2=a2+dela;
    tpx2=x2+r2*cos(ta2);
    tpy2=y2+r2*sin(ta2);
    tl2=sqrt((px1-tpx2)*(px1-tpx2)+(py1-tpy2)*(py1-tpy2))-l12; /* error */
    tl2=abs(tl2);
    ta3=a2-dela;
    tpx3=x2+r2*cos(ta3);
    tpy3=y2+r2*sin(ta3);
    tl3=sqrt((px1-tpx3)*(px1-tpx3)+(py1-tpy3)*(py1-tpy3))-l12; /* error */
    tl3=abs(tl3);
    if(debug>2) printf("tl1=%f, tl2=%f, tl3=%f, a2=%f, dela%f \n", tl1, tl2, tl3, a2, dela);
    if     (tl1<tl2 && tl1<tl3) /* shrink range */
                                dela = dela/2.0;
    else if(tl2<tl1 && tl1<tl3) /* move */
                                a2 = a2+dela;
    else if(tl3<tl1 && tl1<tl2) /* move */
                                a2 = a2-dela;
    else if(tl2<tl3)            /* move */
                                a2 = a2+dela;
    else if(tl3<tl2)            /* move */
                                a2 = a2-dela;
    else  {a2 = a2-dela/2.0; dela = 1.5*dela;}
  }
} /* end min_searcha2 */

static void min_searchoa2()
{
  double tl1, tl2, ta2, tpx2, tpy2, tl3, ta3, tpx3, tpy3;
  double dela = M_PI/60.0;
  int i;
  
  for(i=0; i<12; i++)
  {
    opx2=ox2+or2*cos(oa2);
    opy2=oy2+or2*sin(oa2);
    tl1=sqrt((opx1-opx2)*(opx1-opx2)+(opy1-opy2)*(opy1-opy2))-l12; /* error */
    tl1=abs(tl1);
    if(tl1<0.05) return;
    ta2=oa2+dela;
    tpx2=ox2+or2*cos(ta2);
    tpy2=oy2+or2*sin(ta2);
    tl2=sqrt((opx1-tpx2)*(opx1-tpx2)+(opy1-tpy2)*(opy1-tpy2))-l12; /* error */
    tl2=abs(tl2);
    ta3=oa2-dela;
    tpx3=ox2+or2*cos(ta3);
    tpy3=oy2+or2*sin(ta3);
    tl3=sqrt((opx1-tpx3)*(opx1-tpx3)+(opy1-tpy3)*(opy1-tpy3))-l12; /* error */
    tl3=abs(tl3);
    if(debug>2) printf("tl1=%f, tl2=%f, tl3=%f, a2=%f, dela%f \n", tl1, tl2, tl3, oa2, dela);
    if     (tl1<tl2 && tl1<tl3) /* shrink range */
                                dela = dela/2.0;
    else if(tl2<tl1 && tl1<tl3) /* move */
                                oa2 = oa2+dela;
    else if(tl3<tl1 && tl1<tl2) /* move */
                                oa2 = oa2-dela;
    else if(tl2<tl3)            /* move */
                                oa2 = oa2+dela;
    else if(tl3<tl2)            /* move */
                                oa2 = oa2-dela;
    else  {oa2 = oa2-dela/2.0; dela = 1.5*dela;}
  }
} /* end min_searchoa2 */

static void physicsComputation(void)
{

  /* 120 degree movement, servo level, std angle */
  if(a1<M_PI) a2 = a1/1.5+M_PI/6.0;
  if(a1>=M_PI) a2 = (M_PI-a1)/1.5+M_PI-M_PI/6.0;
  /* 120 degree movement, servo level yet tab turned */
  if(a1<M_PI) a2 = a1/1.5;
  if(a1>=M_PI) a2 = (M_PI-a1)/1.5+M_PI-M_PI/3.0;
  px1=x1+r1*cos(a1);
  py1=y11+r1*sin(a1);

  min_searcha2(); /* find angle a2 */  
  px2=x2+r2*cos(a2);
  py2=y2+r2*sin(a2);
  
  /* 120 degree movement, servo level, std angle */
  if(oa1<M_PI) oa2 = oa1/1.5+M_PI/6.0;
  if(oa1>=M_PI) oa2 = (M_PI-oa1)/1.5+M_PI-M_PI/6.0;
  /* 120 degree movement, servo level yet tab turned */
  if(oa1<M_PI) oa2 = oa1/1.5;
  if(oa1>=M_PI) oa2 = (M_PI-oa1)/1.5+M_PI-M_PI/3.0;
  opx1=ox1+or1*cos(oa1);
  opy1=oy11+or1*sin(oa1);

  min_searchoa2(); /* find angle oa2 */  
  opx2=ox2+or2*cos(oa2);
  opy2=oy2+or2*sin(oa2);
  if(step<1440 && step%45==0)
    printf("a1=%f, a2=%f, cmd=%d, oa1=%f, oa2=%f, cmd=%d \n",
    a1*rtd,  a2*rtd-60.0,  (int)(a2*rtd*255.0/120.0)+1,
    oa1*rtd, oa2*rtd-60.0, (int)(oa2*rtd*255.0/120.0)+1);
  step++;
  
  a1 = a1 + delta;
  if(a1>2.0*M_PI) a1 = 0.0;
  oa1 = oa1 + delta;
  if(oa1>2.0*M_PI) oa1 = 0.0;
  glutPostRedisplay();
} /* end physicsComputation */

static void reshape(int w, int h)
{
  glViewport(0, 0, w, h);
  winWidth = w;
  winHeight = h;
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(0.0, (GLfloat)winWidth, 0.0, (GLfloat)winHeight);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glClearColor(1.0, 1.0, 1.0, 0.0);
} /* end reshape */

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

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

static void display(void)
{
  double tx1, ty1, ta1, tx2, ty2, ta2;
  double otx1, oty1, ota1, otx2, oty2, ota2;
  double lerr;
  int i;
  
  glClear(GL_COLOR_BUFFER_BIT);

  glColor3f(0.0, 0.0, 0.0);
  printstring(200, 50, "servo arms");
  printstring(600, 50, "driven wheel");

  lerr = sqrt((px1-px2)*(px1-px2)+(py1-py2)*(py1-py2));
  lerr = 100.0*(lerr-l12);
  
  glColor3f(1.0, 0.0, 0.0); /* servo arm */
  glLineWidth(3.0);
  glBegin(GL_LINES); 
    glVertex2f(x2, y2);
    glVertex2f(px2, py2);
    glColor3f(0.0, 0.0, 0.0); /* servo center */
    glVertex2f(x2-10.0, y2);
    glVertex2f(x2+10.0, y2);
    glVertex2f(x2, y2-10.0);
    glVertex2f(x2, y2+10.0);
    glColor3f(0.0, 1.0, 0.0); /* connecting rod */
    glVertex2f(px1, py1);
    glVertex2f(px2, py2);
    glColor3f(1.0, 1.0, 0.0); /* length error X100 */
    glVertex2f(400.0+lerr, 100.0);
    glVertex2f(400,        100.0);
  glEnd();

  glColor3f(0.0, 0.0, 1.0); /* servo path */
  glPointSize(1.0);
  glBegin(GL_POINTS);
    ta2=M_PI/6.0; /* normal servo arm angle */
    ta2=0.0; /* for servo arm adjusted */
    for(i=0; i<32; i++)
        {
      tx2 = x2+r2*cos(ta2);
          ty2 = y2+r2*sin(ta2);
      glVertex2f(tx2,ty2);
          ta2 = ta2+M_PI/(3.0*16.0);
        }
  glEnd();

  glColor3f(1.0, 0.0, 0.0); /* wheel position */
  glLineWidth(3.0);
  glBegin(GL_LINES); 
    glVertex2f(x1, y11);
    glVertex2f(px1, py1);
    glColor3f(0.0, 0.0, 0.0); /* servo center */
    glVertex2f(x1-10.0, y11);
    glVertex2f(x1+10.0, y11);
    glVertex2f(x1, y11-10.0);
    glVertex2f(x1, y11+10.0);
  glEnd();

  glColor3f(0.0, 0.0, 1.0); /* wheel path */
  glPointSize(1.0);
  glBegin(GL_POINTS);
    ta1=0.0;
    for(i=0; i<32; i++)
        {
      tx1 = x1+r1*cos(ta1);
          ty1 = y11+r1*sin(ta1);
      glVertex2f(tx1,ty1);
          ta1 = ta1+M_PI/16.0;
        }
  glEnd();
  
  /* now do orthogonal drive */

  glColor3f(1.0, 0.0, 0.0); /* servo arm */
  glLineWidth(3.0);
  glBegin(GL_LINES); 
    glVertex2f(ox2, oy2);
    glVertex2f(opx2, opy2);
    glColor3f(0.0, 0.0, 0.0); /* servo center */
    glVertex2f(ox2-10.0, oy2);
    glVertex2f(ox2+10.0, oy2);
    glVertex2f(ox2, oy2-10.0);
    glVertex2f(ox2, oy2+10.0);
    glColor3f(0.0, 1.0, 0.0); /* connecting rod */
    glVertex2f(opx1, opy1);
    glVertex2f(opx2, opy2);
  glEnd();

  glColor3f(0.0, 0.0, 1.0); /* servo path */
  glPointSize(1.0);
  glBegin(GL_POINTS);
    ota2=M_PI/6.0; /* normal servo arm angle */
    ota2=0.0; /* for servo arm adjusted */
    for(i=0; i<32; i++)
    {
      otx2 = ox2+or2*cos(ota2);
      oty2 = oy2+or2*sin(ota2);
      glVertex2f(otx2,oty2);
      ota2 = ota2+M_PI/(3.0*16.0);
    }
  glEnd();

  glColor3f(1.0, 0.0, 0.0); /* wheel position */
  glLineWidth(3.0);
  glBegin(GL_LINES); 
    glVertex2f(ox1, oy11);
    glVertex2f(opx1, opy1);
    glColor3f(0.0, 0.0, 0.0); /* servo center */
    glVertex2f(ox1-10.0, oy11);
    glVertex2f(ox1+10.0, oy11);
    glVertex2f(ox1, oy11-10.0);
    glVertex2f(ox1, oy11+10.0);
  glEnd();

  glColor3f(0.0, 0.0, 1.0); /* wheel path */
  glPointSize(1.0);
  glBegin(GL_POINTS);
    ota1=0.0;
    for(i=0; i<32; i++)
    {
      otx1 = ox1+or1*cos(ota1);
      oty1 = oy11+or1*sin(ota1);
      glVertex2f(otx1,oty1);
      ota1 = ota1+M_PI/16.0;
    }
  glEnd();
  
  glFlush();  
  glutSwapBuffers();
} /* end display */

int main(int argc, char *argv[])
{
  init();
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
  glutInitWindowSize(winWidth, winHeight);
  glutInitWindowPosition(50,50); 
  glutCreateWindow(argv[0]);
  glutReshapeFunc(reshape);
  glutDisplayFunc(display);
  glutIdleFunc(physicsComputation);
  glutMainLoop();
  return 0;
} /* end wheel.c */