/* robot3.c  based on robot2.c and robot.c and dynamic.c                    */
/*           read robot3.dat and move per commands                          */
/* Interactive Figure Program from Chapter 8 using cylinders (quadrics)     */
/* Style similar to robot program but here we must traverse tree to display */
/* Cylinders are displayed as filled and light/material properties          */
/* are set as in sphere approximation program                               */
/* E. Angel, Interactive Computer Graphics, changes by JSS                  */

/* This version uses a dynamic tree structure */
/* upper and lower limbs have unique colors   */
/* short names for id e.g. TT, TB, HN etc.    */

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

#define TORSO_HEIGHT 5.0
#define UPPER_ARM_HEIGHT 3.0
#define LOWER_ARM_HEIGHT 2.0
#define UPPER_LEG_RADIUS  0.5
#define LOWER_LEG_RADIUS  0.5
#define LOWER_LEG_HEIGHT 2.0
#define UPPER_LEG_HEIGHT 3.0
#define UPPER_LEG_RADIUS  0.5
#define TORSO_RADIUS 1.0
#define UPPER_ARM_RADIUS  0.5
#define LOWER_ARM_RADIUS  0.5
#define HEAD_HEIGHT 1.5
#define HEAD_RADIUS 1.0
#define HAND_HEIGHT 0.6
#define HAND_RADIUS 0.6
#define FOOT_HEIGHT 0.6
#define FOOT_RADIUS 0.6

/* menu select actions  id saved in 'angle' */
#define TORSO_TURN         0
#define TORSO_BEND         1
#define HEAD_NOD           2
#define HEAD_TURN          3
#define RIGHT_UPPER_ARM    4
#define RIGHT_BALL_ARM     5
#define RIGHT_LOWER_ARM    6
#define LEFT_UPPER_ARM     7
#define LEFT_BALL_ARM      8
#define LEFT_LOWER_ARM     9
#define RIGHT_UPPER_LEG   10
#define RIGHT_LOWER_LEG   11
#define LEFT_UPPER_LEG    12
#define LEFT_LOWER_LEG    13
/*                            must be last three */
#define INCREASE_MOVEMENT 14
#define DECREASE_MOVEMENT 15
#define QUIT              16


#define MOVE               0
#define END                1
#define REPEAT             2

typedef struct nid{char nm[4]; int id;  } nid;
static nid ni[17] = {"TT ", TORSO_TURN,
                     "TB ", TORSO_BEND,
                     "HN ", HEAD_NOD,
                     "HT ", HEAD_TURN,
                     "RUA", RIGHT_UPPER_ARM,
                     "RBA", RIGHT_BALL_ARM,
                     "RLA", RIGHT_LOWER_ARM,
                     "LUA", LEFT_UPPER_ARM,
                     "LBA", LEFT_BALL_ARM,
                     "LLA", LEFT_LOWER_ARM,
                     "RUL", RIGHT_UPPER_LEG,
                     "RLL", RIGHT_LOWER_LEG,
                     "LUL", LEFT_UPPER_LEG,
                     "LLL", LEFT_LOWER_LEG,
                     "IM ", INCREASE_MOVEMENT,
                     "DM ", DECREASE_MOVEMENT,
                     "Q  ", QUIT};

/* body part renderers */
static void torso();
static void head();
static void left_upper_arm();
static void right_upper_arm();
static void left_upper_leg();
static void right_upper_leg();
static void hand();
static void foot();
static void move();

typedef float point[3];
typedef struct treenode
{
  GLfloat m[16]; /* 4 by 4 matrix */
  void (*f)();   /* function that renders */
  struct treenode *sibling;
  struct treenode *child;
} treenode, *t_ptr; 


static point rpos = {0.0, 0.0, 0.0}; /* robot base position in x,y,z */
static GLfloat theta[QUIT] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,
        0.0,0.0,0.0,0.0,0.0,0.0,0.0}; /* initial and changed joint angles */
static GLint angle = 0;        /* the angle that will change, see menu id */
static GLfloat movement = 5.0; /* can increase or decrease */

static GLUquadricObj *t, *h, *han, *foo, *lua, *lla, *rua, *rla, *lll,
                     *rll, *rul, *lul;
static double size=1.0; /* scale for lettering */

static t_ptr torso_ptr, head_ptr, hand_ptr, foot_ptr, lua_ptr, rua_ptr,
       lll_ptr, rll_ptr, lla_ptr, rla_ptr, rul_ptr, lul_ptr;

static FILE *inp;
static char buf[100]; /* robot3.dat input */
static int len;       /* includes new line */
static double wt;     /* waste time */

static void traverse(t_ptr root)
{
  if(root==NULL) return;
  glPushMatrix();
  glMultMatrixf(root->m); /* translation, rotation and scaling */
  root->f();              /* execute the function that renders */
  if(root->child!=NULL) traverse(root->child);
  glPopMatrix();
  if(root->sibling!=NULL) traverse(root->sibling);
} /* end traverse */

static void torso()
{
  GLfloat mat_diffuser[]={1.0, 0.0, 0.0, 1.0}; /* red */

  glPushMatrix();
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuser);
  gluCylinder(t,TORSO_RADIUS, TORSO_RADIUS, TORSO_HEIGHT,10,10);
  glPopMatrix();
}

static void head()
{
  GLfloat mat_diffusep[]={1.0, 0.8, 0.8, 1.0}; /* face */

  glPushMatrix();
  glTranslatef(0.0, 0.5*HEAD_HEIGHT,0.0);
  glScalef(HEAD_RADIUS, HEAD_HEIGHT, HEAD_RADIUS);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffusep);
  gluSphere(h,1.0,16,16);
  glPopMatrix();
}

static void left_upper_arm()
{
  GLfloat mat_diffuseg1[]={0.3, 1.0, 0.3, 1.0}; /* green1 */

  glPushMatrix();
  glRotatef(  0.0, 0.0, 1.0, 0.0);
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseg1);
  gluCylinder(lua,UPPER_ARM_RADIUS, UPPER_ARM_RADIUS, UPPER_ARM_HEIGHT,10,10);
  glPopMatrix();
}

static void left_lower_arm()
{
  GLfloat mat_diffuseg2[]={0.6, 1.0, 0.6, 1.0}; /* green2 */

  glPushMatrix();
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseg2);
  gluCylinder(lla,LOWER_ARM_RADIUS, LOWER_ARM_RADIUS, LOWER_ARM_HEIGHT,10,10);
  glPopMatrix();
}

static void hand()
{
  GLfloat mat_diffuseh[]={1.0, 0.9, 0.9, 1.0}; /* hand */

  glPushMatrix();
  glTranslatef(0.0, LOWER_ARM_HEIGHT, 0.0);
  glScalef(HAND_RADIUS, HAND_HEIGHT, HAND_RADIUS);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseh);
  gluSphere(han,1.0,16,16);
  glPopMatrix();
}

static void right_upper_arm()
{
  GLfloat mat_diffuseg1[]={0.3, 1.0, 0.3, 1.0}; /* green1 */

  glPushMatrix();
  glRotatef(  0.0, 0.0, 1.0, 0.0);
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseg1);
  gluCylinder(rua,UPPER_ARM_RADIUS, UPPER_ARM_RADIUS, UPPER_ARM_HEIGHT,10,10);
  glPopMatrix();
}

static void right_lower_arm()
{
  GLfloat mat_diffuseg2[]={0.6, 1.0, 0.6, 1.0}; /* green2 */

  glPushMatrix();
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseg2);
  gluCylinder(rla,LOWER_ARM_RADIUS, LOWER_ARM_RADIUS, LOWER_ARM_HEIGHT,10,10);
  glPopMatrix();
}

static void left_upper_leg()
{
  GLfloat mat_diffuseb2[]={0.3, 0.3, 1.0, 1.0}; /* blue2 */

  glPushMatrix();
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseb2);
  gluCylinder(lul,UPPER_LEG_RADIUS, UPPER_LEG_RADIUS, UPPER_LEG_HEIGHT,10,10);
  glPopMatrix();
}

static void left_lower_leg()
{
  GLfloat mat_diffuseb1[]={0.5, 0.5, 1.0, 1.0}; /* blue1 */

  glPushMatrix();
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseb1);
  gluCylinder(lll,LOWER_LEG_RADIUS, LOWER_LEG_RADIUS, LOWER_LEG_HEIGHT,10,10);
  glPopMatrix();
}

static void foot()
{
  GLfloat mat_diffusef[]={0.7, 0.7, 0.7, 1.0}; /* foot */

  glPushMatrix();
  glTranslatef(0.0, LOWER_LEG_HEIGHT, 0.0);
  glScalef(FOOT_RADIUS, FOOT_HEIGHT, 2.0*FOOT_RADIUS);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffusef);
  gluSphere(foo,1.0,16,16);
  glPopMatrix();
}

static void right_upper_leg()
{
  GLfloat mat_diffuseb2[]={0.3, 0.3, 1.0, 1.0}; /* blue2 */

  glPushMatrix();
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseb2);
  gluCylinder(rul,UPPER_LEG_RADIUS, UPPER_LEG_RADIUS, UPPER_LEG_HEIGHT,10,10);
  glPopMatrix();
}

static void right_lower_leg()
{
  GLfloat mat_diffuseb1[]={0.5, 0.5, 1.0, 1.0}; /* blue */

  glPushMatrix();
  glRotatef(-90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuseb1);
  gluCylinder(rll,LOWER_LEG_RADIUS, LOWER_LEG_RADIUS, LOWER_LEG_HEIGHT,10,10);
  glPopMatrix();
}

static void instructions()
{
  char * p;
  char msg[] = "middle mouse for menu selection";
  char msg2[] = "then right or left click to move";
  char msg3[] = "keyboard +, -, x, X, y, Y moves";
  GLfloat blk[] = {0.0, 0.0, 0.0, 1.0};

  glPushMatrix(); /* display instructions */
    glLoadIdentity ();
    glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, blk); 
    glEnable(GL_LINE_SMOOTH);
    glEnable(GL_BLEND);
    glTranslatef(-8.0, -7.5, 0.0);
    glScalef(size/150.0, size/150.0, 0.0);
    for(p=msg; *p; p++)
      glutStrokeCharacter(GLUT_STROKE_ROMAN, *p);
    glLoadIdentity ();
    glTranslatef(-8.0, -8.5, 0.0);
    glScalef(size/150.0, size/150.0, 0.0);
    for(p=msg2; *p; p++)
      glutStrokeCharacter(GLUT_STROKE_ROMAN, *p);
    glLoadIdentity ();
    glTranslatef(-8.0, -9.5, 0.0);
    glScalef(size/150.0, size/150.0, 0.0);
    for(p=msg3; *p; p++)
      glutStrokeCharacter(GLUT_STROKE_ROMAN, *p);
  glPopMatrix();
} /* end instructions */

static void display(void)
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glLoadIdentity();
  
  traverse(torso_ptr); /* render the robot */

  instructions();
  glFlush();
  glutSwapBuffers();
} /* end display */

static void mouse(int btn, int state, int x, int y)
{
  if(btn==GLUT_LEFT_BUTTON && state == GLUT_DOWN) 
  {
    theta[angle] += movement;
    if( theta[angle] > 360.0 ) theta[angle] -= 360.0;
  }
  if(btn==GLUT_RIGHT_BUTTON && state == GLUT_DOWN) 
  {
    theta[angle] -= movement;
    if( theta[angle] < 360.0 ) theta[angle] += 360.0;
  }
  move();
} /* end mouse */

static void move()
{
  glPushMatrix();
  switch(angle)
  {
    case TORSO_TURN :
    case TORSO_BEND :
      glLoadIdentity();
      glRotatef(theta[TORSO_TURN], 0.0, 1.0, 0.0);
      glRotatef(theta[TORSO_BEND], 1.0, 0.0, 0.0);
      glTranslatef(rpos[0], rpos[1], rpos[2]);
      glGetFloatv(GL_MODELVIEW_MATRIX,torso_ptr->m);
      break;
    case HEAD_NOD :
    case HEAD_TURN :
      glLoadIdentity();
      glTranslatef(0.0, TORSO_HEIGHT+0.5*HEAD_HEIGHT, 0.0);
      glRotatef(theta[HEAD_NOD], 1.0, 0.0, 0.0);
      glRotatef(theta[HEAD_TURN], 0.0, 1.0, 0.0);
      glTranslatef(0.0, -0.5*HEAD_HEIGHT, 0.0);
      glGetFloatv(GL_MODELVIEW_MATRIX,head_ptr->m);
      break;
   case LEFT_BALL_ARM :
   case LEFT_UPPER_ARM :
     glLoadIdentity();
     glTranslatef(-(TORSO_RADIUS+UPPER_ARM_RADIUS), 0.9*TORSO_HEIGHT, 0.0);
     glRotatef(theta[LEFT_BALL_ARM], 0.0, 0.0, 1.0);
     glRotatef(theta[LEFT_UPPER_ARM], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,lua_ptr->m);
     break;
   case RIGHT_BALL_ARM :
   case RIGHT_UPPER_ARM :
     glLoadIdentity();
     glTranslatef(TORSO_RADIUS+UPPER_ARM_RADIUS, 0.9*TORSO_HEIGHT, 0.0);
     glRotatef(theta[RIGHT_BALL_ARM], 0.0, 0.0, 1.0);
     glRotatef(theta[RIGHT_UPPER_ARM], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,rua_ptr->m);
     break;
   case RIGHT_UPPER_LEG :
     glLoadIdentity();
     glTranslatef(TORSO_RADIUS+UPPER_LEG_RADIUS, 0.1*UPPER_LEG_HEIGHT, 0.0);
     glRotatef(theta[RIGHT_UPPER_LEG], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,rul_ptr->m);
     break;
   case LEFT_UPPER_LEG :
     glLoadIdentity();
     glTranslatef(-(TORSO_RADIUS+UPPER_LEG_RADIUS), 0.1*UPPER_LEG_HEIGHT, 0.0);
     glRotatef(theta[LEFT_UPPER_LEG], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,lul_ptr->m);
     break;
   case LEFT_LOWER_ARM :
     glLoadIdentity();
     glTranslatef(0.0, UPPER_ARM_HEIGHT, 0.0);
     glRotatef(theta[LEFT_LOWER_ARM], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,lla_ptr->m);
     break;
   case LEFT_LOWER_LEG :
     glLoadIdentity();
     glTranslatef(0.0, UPPER_LEG_HEIGHT, 0.0);
     glRotatef(theta[LEFT_LOWER_LEG], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,lll_ptr->m);
     break;
   case RIGHT_LOWER_LEG :
     glLoadIdentity();
     glTranslatef(0.0, UPPER_LEG_HEIGHT, 0.0);
     glRotatef(theta[RIGHT_LOWER_LEG], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,rll_ptr->m);
     break;
   case RIGHT_LOWER_ARM :
     glLoadIdentity();
     glTranslatef(0.0, UPPER_ARM_HEIGHT, 0.0);
     glRotatef(theta[RIGHT_LOWER_ARM], 1.0, 0.0, 0.0);
     glGetFloatv(GL_MODELVIEW_MATRIX,rla_ptr->m);
     break;
   default :
     break;
  }
  glPopMatrix();
  glutPostRedisplay();
} /* end move */

static void keyboard(unsigned char key, int x, int y)
{
  switch (key)
  {
    case '+':
      theta[angle] += movement;
      if( theta[angle] > 360.0 ) theta[angle] -= 360.0;
      break;
    case '-':
      theta[angle] -= movement;
      if( theta[angle] < 360.0 ) theta[angle] += 360.0;
      break;
    case 'x':
      rpos[0] -= movement/10.0;
      angle=TORSO_TURN;
      break;
    case 'X':
      rpos[0] += movement/10.0;
      angle=TORSO_TURN;
      break;
    case 'y':
      rpos[1] -= movement/10.0;
      angle=TORSO_TURN;
      break;
    case 'Y':
      rpos[1] += movement/10.0;
      angle=TORSO_TURN;
      break;
    default :
      break;
  }
  move();
} /* end keyboard */

static void menu(int id)
{
  if(id < INCREASE_MOVEMENT) angle=id; /* set up for next right or left mouse click */
  else if(id == INCREASE_MOVEMENT) movement *= 1.5;
  else if(id == DECREASE_MOVEMENT) movement /= 1.5;
  else if(id == QUIT) exit(0);
} /* end menu */

void myReshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  if (w <= h)
    glOrtho(-10.0, 10.0, -10.0 * (GLfloat) h / (GLfloat) w,
            10.0 * (GLfloat) h / (GLfloat) w, -10.0, 10.0);
  else
    glOrtho(-10.0 * (GLfloat) w / (GLfloat) h,
            10.0 * (GLfloat) w / (GLfloat) h, 0.0, 10.0, -10.0, 10.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
} /* end myReshape */

static void myinit()
{
  int i;

  GLfloat mat_specular[]={1.0, 1.0, 1.0, 1.0};
  GLfloat mat_diffuse[]={1.0, 0.0, 0.0, 1.0}; /* red material*/
  GLfloat mat_ambient[]={1.0, 1.0, 1.0, 1.0};
  GLfloat mat_shininess={100.0};
  GLfloat light_ambient[]={0.0, 0.0, 0.0, 1.0};
  GLfloat light_diffuse[]={1.0, 1.0, 1.0, 1.0}; /* white light */
  GLfloat light_specular[]={1.0, 1.0, 1.0, 1.0};
  GLfloat light_position[]={10.0, 10.0, 10.0, 0.0};

  for(i=0; i<17; i++) theta[i] = 0.0;
  theta[LEFT_UPPER_LEG]  = 180.0; /* legs normally down */
  theta[RIGHT_UPPER_LEG] = 180.0;
  
  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
  glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
  glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
  glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);

  glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
  glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
  glMaterialf(GL_FRONT, GL_SHININESS, mat_shininess);

  glShadeModel(GL_SMOOTH);
  glEnable(GL_LIGHTING); 
  glEnable(GL_LIGHT0);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_DEPTH_TEST); 
  glClearColor(1.0, 1.0, 1.0, 1.0);
  glColor3f(1.0, 0.0, 0.0);
  /* allocate quadrics with filled drawing style */
  h=gluNewQuadric(); /* head */
  gluQuadricDrawStyle(h, GLU_FILL);
  han=gluNewQuadric(); /* hand */
  gluQuadricDrawStyle(h, GLU_FILL);
  foo=gluNewQuadric(); /* foot */
  gluQuadricDrawStyle(h, GLU_FILL);
  t=gluNewQuadric(); /* torso */
  gluQuadricDrawStyle(t, GLU_FILL);
  lua=gluNewQuadric(); /* left upper arm, etc. */
  gluQuadricDrawStyle(lua, GLU_FILL);
  lla=gluNewQuadric();
  gluQuadricDrawStyle(lla, GLU_FILL);
  rua=gluNewQuadric();
  gluQuadricDrawStyle(rua, GLU_FILL);
  rla=gluNewQuadric();
  gluQuadricDrawStyle(rla, GLU_FILL);
  lul=gluNewQuadric();
  gluQuadricDrawStyle(lul, GLU_FILL);
  lll=gluNewQuadric();
  gluQuadricDrawStyle(lll, GLU_FILL);
  rul=gluNewQuadric();
  gluQuadricDrawStyle(rul, GLU_FILL);
  rll=gluNewQuadric();
  gluQuadricDrawStyle(rll, GLU_FILL);

  /* Set up tree */
  torso_ptr =  malloc(sizeof(treenode));
  head_ptr = malloc(sizeof(treenode));
  hand_ptr = malloc(sizeof(treenode)); /* leaf */
  foot_ptr = malloc(sizeof(treenode)); /* leaf */
  lua_ptr = malloc(sizeof(treenode));
  rua_ptr = malloc(sizeof(treenode));
  lll_ptr = malloc(sizeof(treenode));
  rll_ptr = malloc(sizeof(treenode));
  lla_ptr = malloc(sizeof(treenode));
  rla_ptr = malloc(sizeof(treenode));
  rul_ptr = malloc(sizeof(treenode));
  lul_ptr = malloc(sizeof(treenode));

  glLoadIdentity();
  glRotatef(theta[TORSO_TURN], 0.0, 1.0, 0.0);
  glRotatef(theta[TORSO_BEND], 1.0, 1.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,torso_ptr->m);
  torso_ptr->f = torso;
  torso_ptr->sibling = NULL;
  torso_ptr->child =  head_ptr;

  glLoadIdentity();
  glTranslatef(0.0, TORSO_HEIGHT+0.5*HEAD_HEIGHT, 0.0);
  glRotatef(theta[HEAD_NOD], 1.0, 0.0, 0.0);
  glRotatef(theta[HEAD_TURN], 0.0, 1.0, 0.0);
  glTranslatef(0.0, -0.5*HEAD_HEIGHT, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,head_ptr->m);
  head_ptr->f = head;
  head_ptr->sibling = lua_ptr;
  head_ptr->child = NULL;

  glLoadIdentity();
  glTranslatef(-(TORSO_RADIUS+UPPER_ARM_RADIUS), 0.9*TORSO_HEIGHT, 0.0);
  glRotatef(theta[LEFT_BALL_ARM], 0.0, 0.0, 1.0);
  glRotatef(theta[LEFT_UPPER_ARM], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,lua_ptr->m);
  lua_ptr->f = left_upper_arm;
  lua_ptr->sibling =  rua_ptr;
  lua_ptr->child = lla_ptr;

  glLoadIdentity();
  glTranslatef(TORSO_RADIUS+UPPER_ARM_RADIUS, 0.9*TORSO_HEIGHT, 0.0);
  glRotatef(theta[RIGHT_BALL_ARM], 0.0, 0.0, 1.0);
  glRotatef(theta[RIGHT_UPPER_ARM], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,rua_ptr->m);
  rua_ptr->f = right_upper_arm;
  rua_ptr->sibling =  lul_ptr;
  rua_ptr->child = rla_ptr;

  glLoadIdentity();
  glTranslatef(-(TORSO_RADIUS+UPPER_LEG_RADIUS), 0.1*UPPER_LEG_HEIGHT, 0.0);
  glRotatef(theta[LEFT_UPPER_LEG], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,lul_ptr->m);
  lul_ptr->f = left_upper_leg;
  lul_ptr->sibling =  rul_ptr;
  lul_ptr->child = lll_ptr;

  glLoadIdentity();
  glTranslatef(TORSO_RADIUS+UPPER_LEG_RADIUS, 0.1*UPPER_LEG_HEIGHT, 0.0);
  glRotatef(theta[RIGHT_UPPER_LEG], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,rul_ptr->m);
  rul_ptr->f = right_upper_leg;
  rul_ptr->sibling =  NULL;
  rul_ptr->child = rll_ptr;

  glLoadIdentity();
  glTranslatef(0.0, UPPER_ARM_HEIGHT, 0.0);
  glRotatef(theta[LEFT_LOWER_ARM], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,lla_ptr->m);
  lla_ptr->f = left_lower_arm;
  lla_ptr->sibling =  NULL;
  lla_ptr->child = hand_ptr;

  glLoadIdentity();
  glTranslatef(0.0, UPPER_ARM_HEIGHT, 0.0);
  glRotatef(theta[RIGHT_LOWER_ARM], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,rla_ptr->m);
  rla_ptr->f = right_lower_arm;
  rla_ptr->sibling =  NULL;
  rla_ptr->child = hand_ptr;

  glLoadIdentity();
  glTranslatef(0.0, UPPER_LEG_HEIGHT, 0.0);
  glRotatef(theta[LEFT_LOWER_LEG], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,lll_ptr->m);
  lll_ptr->f = left_lower_leg;
  lll_ptr->sibling =  NULL;
  lll_ptr->child = foot_ptr;

  glLoadIdentity();
  glTranslatef(0.0, FOOT_HEIGHT, 0.0);
  glRotatef(0.0, 1.0, 0.0, 0.0); /* ? */
  glGetFloatv(GL_MODELVIEW_MATRIX,foot_ptr->m);
  foot_ptr->f = foot;
  foot_ptr->sibling =  NULL;
  foot_ptr->child = NULL;

  glLoadIdentity();
  glTranslatef(0.0, HAND_HEIGHT, 0.0);
  glRotatef(0.0, 1.0, 0.0, 0.0); /* ? */
  glGetFloatv(GL_MODELVIEW_MATRIX,hand_ptr->m);
  hand_ptr->f = hand;
  hand_ptr->sibling =  NULL;
  hand_ptr->child = NULL;

  glLoadIdentity();
  glTranslatef(0.0, UPPER_LEG_HEIGHT, 0.0);
  glRotatef(theta[RIGHT_LOWER_LEG], 1.0, 0.0, 0.0);
  glGetFloatv(GL_MODELVIEW_MATRIX,rll_ptr->m);
  rll_ptr->f = right_lower_leg;
  rll_ptr->sibling =  NULL;
  rll_ptr->child = foot_ptr;

  glLoadIdentity();
} /* end myinit */

static int get_int(char buff[], int *i)
{
  char num[15];
  int j=0, k;

  if(buff[*i]=='\n') return -99;
  while(buff[*i]==' ')
  {
    (*i)++;
  }
  if(buff[*i]=='\n') return -99;
  while(buff[*i]!=' ')
  {
    num[j]=buff[*i];
    j++;
    (*i)++;
  }
  num[j]='\0'; /* terminate string */
  k= atoi(num);
  printf("get_int returns %d \n",k);
  return k;
} /* end get_int */

static int get_mode(char buff[], int *i)
{
  char num[15];
  int j=0, k;

  if(buff[*i]=='\n') return -99;
  while(buff[*i]==' ') {(*i)++;}
  if(buff[*i]=='\n') return -99;
  while(buff[*i]!=' ' && buff[*i]!='\n')
  {
    num[j]=buff[*i]; j++; (*i)++;
  }
  k=-99;
  if(!strncmp("move",num,4)) k=0;
  if(!strncmp("end",num,3)) k=1;
  if(!strncmp("repeat",num,6)) k=2;
  printf("get_mode returns %d \n",k);
    
  return k;
} /* end get_mode */

static int get_id(char buff[], int *i)
{
  char num[15];
  int j=0, k, m;

  num[1]=' ';
  num[2]=' ';
  if(buff[*i]=='\n') return -99;
  while(buff[*i]==' ') {(*i)++;}
  if(buff[*i]=='\n') return -99;
  while(buff[*i]!=' ') {num[j]=buff[*i]; j++; (*i)++;}
  for(k=0; k<=QUIT; k++)
  {
    if(!strncmp(ni[k].nm,num,3)) {m=ni[k].id; break;}
  }
  if(k==QUIT+1)
  {
    printf("BAD ID buff=%s \n", buff);
    /* exit(1); */
  }
  printf("get_id returns %d \n",m);
  return m;
} /* end get_id */

static float get_float(char buff[], int *i)
{
  float x=0.0;
  int j;

  if(buff[*i]=='\n') return -99.0;
  while(buff[*i]==' ') {(*i)++;}
  if(buff[*i]=='\n') return -99.0;
  j=*i;
  while(buff[*i]!=' ') {(*i)++;}
  sscanf(&buff[j],"%f",&x);
  printf("get_float returns %f\n", x);
  return x;
} /* end get_float */


static void parse(int* seq, int* mode, float* dt, float* dx, float*dy,
                  int* nj, int* j, float* ang)
{
  static int i;

  printf("enter parse i=%d,  buf=%s \n", i, buf);

  while(buf[0]=='#')
  {
    fgets(buf, 99, inp);
    printf("fgets %s \n", buf);
    i=0;
    *nj=0;
  }
  if((*nj)>0)
  {
    /* process next j and ang */
    printf("parse getting next j,ang \n");
    (*nj)--; /* did one */
    *j=get_id(buf, &i);
    *ang=get_float(buf, &i);
    return;
  }
  else
  {
    i=0; /* static, must be managed here */
  }
  *seq=get_int(buf, &i);
  *mode=get_mode(buf, &i);
  *dt=get_float(buf, &i);
  *dx=get_float(buf, &i);
  *dy=get_float(buf, &i);
  *nj=get_int(buf, &i);
  (*nj)--; /* did one */
  *j=get_id(buf, &i);
  *ang=get_float(buf, &i);
  printf("parse returns seq=%d, mode=%d, dt=%f, dx=%f, dy=%f, nj=%d, j=%d, ang=%f \n",
         *seq, *mode, *dt, *dx, *dy, *nj, *j, *ang);

} /* end parse */

static void get_command(void)
{
  int seq, mode, nj, id;
  float dt, dx, dy, ang;
  double t;

  wt=0.0;
  fgets(buf, 99, inp);
  printf("buf=%s", buf);
  nj=0;
  parse(&seq, &mode, &dt, &dx, &dy, &nj, &id, &ang);
 
  printf("about to do mode %d \n", mode);
  switch(mode) /* very little error checking */
  {
    case MOVE:
      /* set theta[id]+=ang  or  rpos[0]+=dx rpos[1]+=dy */
      angle=id;
      theta[angle]+=ang;
      rpos[0]+=dx;
      rpos[1]+=dy;
      move(); /* then this function gets called again, workproc */
      glutPostRedisplay();
      if((dx!=0.0 || dy!=0.0) && id!=TORSO_TURN && id!=TORSO_BEND)
      {
        angle=TORSO_TURN;
        move();
      }
      break;
    case END:
      nj=0;
      glutPostRedisplay();
      glutIdleFunc (NULL);
      break;
    case REPEAT:
      nj=0;
      break;
    default:
      nj=0;
      break;
  }
  while(nj>0) /* possibly more moves */
  {
    parse(&seq, &mode, &dt, &dx, &dy, &nj, &id, &ang);
    angle=id;  /* only nj, id and ang should change */
    theta[angle]+=ang;
    move();
  }
  glutPostRedisplay();
  for(t=0.0; t<dt; t+=0.000001) {wt+=sin(cos(sin(cos(sin(cos(t))))));}
} /* end get_command */

int main(int argc, char *argv[])
{
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowSize(500, 550);
  glutCreateWindow(argv[0]);
  myinit();
  glutReshapeFunc(myReshape);
  glutDisplayFunc(display);
  glutMouseFunc(mouse);
  glutKeyboardFunc(keyboard);
  glutCreateMenu(menu);
  glutAddMenuEntry("torso turn",      TORSO_TURN);
  glutAddMenuEntry("torso bend",      TORSO_BEND);
  glutAddMenuEntry("head nod",        HEAD_NOD);
  glutAddMenuEntry("head turn",       HEAD_TURN);
  glutAddMenuEntry("right upper arm", RIGHT_UPPER_ARM);
  glutAddMenuEntry("right ball joint arm", RIGHT_BALL_ARM);
  glutAddMenuEntry("right lower arm", RIGHT_LOWER_ARM);
  glutAddMenuEntry("left upper arm",  LEFT_UPPER_ARM);
  glutAddMenuEntry("left ball joint arm",  LEFT_BALL_ARM);
  glutAddMenuEntry("left lower arm",  LEFT_LOWER_ARM);
  glutAddMenuEntry("right upper leg", RIGHT_UPPER_LEG);
  glutAddMenuEntry("right lower leg", RIGHT_LOWER_LEG);
  glutAddMenuEntry("left upper leg",  LEFT_UPPER_LEG);
  glutAddMenuEntry("left lower leg",  LEFT_LOWER_LEG);
  glutAddMenuEntry("increase movement", INCREASE_MOVEMENT);
  glutAddMenuEntry("decrease movement", DECREASE_MOVEMENT);
  glutAddMenuEntry("quit", QUIT);
  glutAttachMenu(GLUT_MIDDLE_BUTTON);
  if(argc<1)
  {
    printf("provide a file name such as robot3.dat \n");
    exit(1);
  }
  inp=fopen(argv[1]  ,"r");
  if(inp==NULL)
  {
    printf("can not open %s data control file \n", argv[0]);
  }
  else
  {
    fgets(buf, 99, inp);
    len=strlen(buf);
    printf("read %d char into buf=%s \n",len, buf);
    glutIdleFunc (get_command);
  }
  glutMainLoop();
  return 0;
} /* end main robot3 */