/* Robot arm demo program */
/* Written by H. Kenn */
/* Some basic stuff taken from glutmech.cpp */

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

#define VIEW_TURN_RATE 5
#define JOINT_TURN_RATE 5

int turn;
int turn1;

int dof[5];
int doflim1[5] = {0,0,0,0,0};
int doflim2[5] = {360,90,170,170,360};


void
TurnRight(void)
{
  turn = (turn - VIEW_TURN_RATE) % 360;
}

void
TurnLeft(void)
{
  turn = (turn + VIEW_TURN_RATE) % 360;
}

void
TurnForwards(void)
{
  turn1 = (turn1 - VIEW_TURN_RATE) % 360;
}

void
TurnBackwards(void)
{
  turn1 = (turn1 + VIEW_TURN_RATE) % 360;
}

void
Box(float width, float height, float depth)
{
  char i, j = 0;
  glColor3f(1,0,0);
  glPushMatrix();
  glScalef(width,height,depth);
  glutWireCube(1.0);
  glPopMatrix();

}

void
Joint()
{
  glColor3f(1,1,0);
  glPushMatrix();
  GLUquadricObj * qobj;
  qobj=gluNewQuadric();
  gluQuadricDrawStyle(qobj,GLU_LINE);
  glRotatef(90,0,1,0);
  glTranslatef(0,0,-0.15);
  gluCylinder(qobj,0.1,0.1,0.3,8,8);
  glPopMatrix();
}


void
Arm()
{
  glPushMatrix();
  Box(1,0.2,1);
  glTranslatef(0,0.2,0); /* on top of the box */
  glRotatef(dof[0],0,1,0);/* rotate around the y axis */
  glRotatef(dof[1],1,0,0);/* rotate around the x axis */
  Joint();
  glTranslatef(0,0.5,0); /* move to the middle of the box*/
  Box(0.2,1,0.2); /* draw a box */
  glTranslatef(0,0.5,0); /* move to the end of the box */
  glRotatef(dof[2],1,0,0); /* rotate elbow joint */
  Joint();
  glTranslatef(0,0.5,0); /* move to the middle of the box*/
  Box(0.2,1,0.2); /* draw a box */
  glTranslatef(0,0.5,0); /* move to the end of the box */
  glRotatef(dof[3],1,0,0); /* rotate hand tilt joint */
  Joint();
  glRotatef(dof[4],0,1,0); /* rotate hand turn joint */
  glTranslatef(0,0.1,0); /* move to the middle of the box*/
  Box(0.2,0.2,0.2); /* draw a box */
  glTranslatef(0,0.1,0); /* move to the end of the box */
  glTranslatef(0,0.1,0); /* move to the end of the box */
  glRotatef(90,0,0,1);
  glRotatef(-90,1,0,0);
  glColor3f(1,1,1);
  glutWireTeapot(0.1);
  glPopMatrix();
 



}


void
display(void)
{
  glClearColor(0.0, 0.0, 0.0, 0.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  //  glEnable(GL_DEPTH_TEST);

  glPushMatrix();
  glRotatef((GLfloat) turn, 0.0, 1.0, 0.0);
  glRotatef((GLfloat) turn1, 1.0, 0.0, 0.0);

  glPushMatrix();
  //glutWireTeapot(1);
  //Box(1,2,1);
  Arm();
  glPopMatrix();

  glPopMatrix();
  glFlush();
  glutSwapBuffers();
}

void
myinit(void)
{

  for (int i=0;i<5;i++) dof[i]=0;


}

void
myReshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glOrtho(-3,3,-3*((GLfloat)h/(GLfloat)w),3*((GLfloat)h/(GLfloat)w),0,20);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  gluLookAt(-10,1.5,0,
            10,1.5,0,
	    0,1,0);
}

void doturn(int axis,int dir)
{

  if (axis<0 || axis>4) return;

  if (dir==1){
    dof[axis]+=JOINT_TURN_RATE;
  } 
  else{
    dof[axis]-=JOINT_TURN_RATE;
  }

  if(dof[axis]<doflim1[axis])dof[axis]=doflim1[axis];
  if(dof[axis]>doflim2[axis])dof[axis]=doflim2[axis];


  while( dof[axis]>359) dof[axis]-=360;
  while( dof[axis]<0) dof[axis]+=360;


}

void
keyboard(unsigned char key, int x, int y)
{

  int i = 0;

  switch (key) {
    /* start arm control functions */
  case '1':
    doturn(0,1);
      i++;
    break;
  case '2':
    doturn(1,1);
      i++;
    break;
  case '3':
    doturn(2,1);
       i++;
    break;
  case '4':
    doturn(3,1);
       i++;
    break;
  case '5':
    doturn(4,1);
       i++;
       break;
  case '!':
    doturn(0,-1);
      i++;
    break;
  case '@':
    doturn(1,-1);
      i++;
    break;
  case '#':
    doturn(2,-1);
       i++;
    break;
  case '$':
    doturn(3,-1);
       i++;
    break;
  case '%':
    doturn(4,-1);
       i++;
    break;
  }
  if (i)
    glutPostRedisplay();
}

void
special(int key, int x, int y)
{

  int i = 0;

  switch (key) {
    /* start of view position functions */
  case GLUT_KEY_RIGHT:{
      TurnRight();
      i++;
    }
    break;
  case GLUT_KEY_LEFT:{
      TurnLeft();
      i++;
    }
    break;
  case GLUT_KEY_DOWN:{
      TurnForwards();
      i++;
    }
    break;
  case GLUT_KEY_UP:{
      TurnBackwards();
      i++;
    }
    break;
    /* end of view postions functions */
  }

  if (i)
    glutPostRedisplay();
}

void
menu_select(int mode)
{
  switch (mode) {
  case 1:
    exit(EXIT_SUCCESS);
  }
}

/* ARGSUSED */
void
null_select(int mode)
{
}

void
glutMenu(void)
{

  glutCreateMenu(menu_select);
  glutAddMenuEntry("Quit", 1);
  glutAttachMenu(GLUT_LEFT_BUTTON);
  glutAttachMenu(GLUT_RIGHT_BUTTON);
}

int
main(int argc, char **argv)
{
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
  glutInitWindowSize(800, 600);
  glutCreateWindow("Robot Arm");
  myinit();
  glutDisplayFunc(display);
  glutReshapeFunc(myReshape);
  glutKeyboardFunc(keyboard);
  glutSpecialFunc(special);
  glutMenu();
  glutMainLoop();
  return 0;             /* ANSI C requires main to return int. */
}