#include <iostream.h>
#include <list.h> 
#include <stdlib.h>
#include <unistd.h>
#include <GL/glut.h>
#include <Message.h>
#include "GL_Server.h"
#include "GL_Common.h"
#include "GL_Robot.h"
#include <Command_Type.h>
#include "Drawing_Routines.h"
#include "Robot_Structure.h"
#include "Small_Gripper_Structure.h"
#include "Big_Gripper_Structure.h"
#include "Vacuum_Gripper_Structure.h"
#include "Environment.h"
#include "GL_Matrix.h"
#include "Calculations.h"

#define RAD_TO_DEG 57.295779513 

static GL_Robot* robot_ptr;
static Small_Gripper* smallgrip_ptr;
static Big_Gripper* biggrip_ptr;
static Vacuum_Gripper* vgrip_ptr;
static Environment* env_ptr;  
static Message* msg_ptr;  

static int view_mode = ROTATE_VIEW;
static int cursor_x, cursor_y;
static float view_y = 0;
static float view_z = 10;
static float shift_x = -300;
static float shift_y = 0;
static float scale = 1.0;
static int current_joint = 0;  
static int joints_mode = JOINT;
static int viewtable = 0;
static char oldkey = 0;
static int colortype = 0; // r,g,b == 0,1,2
static int colormode = FLOOR_COLOR;

static GLfloat red[4] = { 1.0, 0.0, 0.0, 1.0 };
//static GLfloat green[4] = {0.0, 1.0, 0.0, 1.0 };
static GLfloat light_green[4] = {0.4, 0.9, 0.4, 1.0 };
static GLfloat blue[4] = {0.0, 0.0, 1.0, 1.0 };
static GLfloat light_blue[4] = {0.5, 0.5, 1.0, 1.0 };
static GLfloat orange[4] = { 1.0, 0.5, 0.0, 1.0 };
static GLfloat yellow[4] = { 1.0, 1.0 , 0.0, 1.0 };
static GLfloat brown[4] = { 0.48, 0.40 , 0.31, 1.0 };
static GLfloat black[4] = { 0.2, 0.2 , 0.2, 1.0 };
static GLfloat cellfloor[4] = { 0.2, 0.5 , 0.6, 1.0 };
static GLfloat grey[4] = { 0.6, 0.6 , 0.6, 1.0 };
static GLfloat darkgrey[4] = { 0.3, 0.3 , 0.3, 1.0 };
static GLfloat cellbackground[4] ={ 0.0, 0.0, 0.0, 1.0};

GLfloat light_position[] = { 1.0, 1.0, 2.0, 0.0 };


void draw_robot()
{
  /* base */
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, orange);
  draw_cilinder( robot_ptr->base0_R, 
		 robot_ptr->base0_H, 
		 robot_ptr->base0_N, 
		 robot_ptr->base0_pos );
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, brown);
  draw_cilinder( robot_ptr->base1_R, 
		 robot_ptr->base1_H, 
		 robot_ptr->base1_N, 
		 robot_ptr->base1_pos );
  //link1
  glRotatef( (GLfloat) (robot_ptr->joints[1] * RAD_TO_DEG),
	     0.0, 0.0, 1.0);
  glTranslatef( 0, 0, robot_ptr->ljoints[0]);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, black);
  draw_box(robot_ptr->box1_1, robot_ptr->box1_1_pos);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, orange);
  draw_box(robot_ptr->box1_2, robot_ptr->box1_2_pos);
  draw_cilinder( robot_ptr->cil1_1_R, 
		 robot_ptr->cil1_1_H, 
		 robot_ptr->cil1_1_N, 
		 robot_ptr->cil1_1_pos );
  glTranslatef( robot_ptr->ljoints[1], 0, 0);
  glPushMatrix();
  glRotatef( 90.0, 1.0, 0.0, 0.0);
  draw_cilinder( robot_ptr->cil1_2_R, 
		 robot_ptr->cil1_2_H, 
		 robot_ptr->cil1_2_N, 
		 robot_ptr->cil1_2_pos );
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, black);
  draw_box(robot_ptr->box1_3, robot_ptr->box1_3_pos);
  glPopMatrix();

  // link 2
  glRotatef( (GLfloat) (robot_ptr->joints[2] * RAD_TO_DEG ), 
	     0.0, 1.0, 0.0 );
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, orange);
  draw_box(robot_ptr->box2_1, robot_ptr->box2_1_pos);
  draw_box(robot_ptr->box2_2, robot_ptr->box2_2_pos);
 
  // link 3
  glTranslatef( 0, 0, robot_ptr->ljoints[2]);
  glRotatef(  (GLfloat) (robot_ptr->joints[3] * RAD_TO_DEG), 
	      0.0, 1.0, 0.0 );
  glPushMatrix();
  glRotatef( 90.0, 1.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, orange);
  draw_cilinder( robot_ptr->cil3_R, 
		 robot_ptr->cil3_H, 
		 robot_ptr->cil3_N, 
		 robot_ptr->cil3_pos );
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, black);
  draw_box(robot_ptr->box3_1, robot_ptr->box3_1_pos);
  glPopMatrix();
  glTranslatef( 0, 0, robot_ptr->ljoints[3]);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, orange);
  draw_box(robot_ptr->box3_2, robot_ptr->box3_2_pos);

  // link 4 
  glTranslatef( robot_ptr->ljoints[4], 0, 0);
  glRotatef( (GLfloat) (robot_ptr->joints[4] * RAD_TO_DEG), 
	     1.0, 0.0, 0.0 );
  glPushMatrix();
  glRotatef( 90.0, 0.0, 1.0, 0.0);
  draw_cilinder( robot_ptr->cil4_1_R, 
		 robot_ptr->cil4_1_H, 
		 robot_ptr->cil4_1_N, 
		 robot_ptr->cil4_1_pos );
  glPopMatrix();
  draw_box(robot_ptr->box4, robot_ptr->box4_pos);
  glTranslatef(robot_ptr->ljoints[5], 0, 0);
  glPushMatrix();
  glRotatef( 90.0, 1.0, 0.0, 0.0);
  draw_cilinder( robot_ptr->cil4_2_R, 
		 robot_ptr->cil4_2_H, 
		 robot_ptr->cil4_2_N, 
		 robot_ptr->cil4_2_pos );
  glPopMatrix();

  // link 5
  glRotatef( (GLfloat) (robot_ptr->joints[5] * RAD_TO_DEG), 
	     0.0, 1.0, 0.0 );
  draw_box(robot_ptr->box5, robot_ptr->box5_pos);

  // link 6
  glTranslatef(robot_ptr->ljoints[6], 0, 0);
  glRotatef( (GLfloat) (robot_ptr->joints[6] * RAD_TO_DEG),
	     1.0, 0.0, 0.0 );
  glPushMatrix();
  glRotatef( 90.0, 0.0, 1.0, 0.0);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, grey);
  draw_cilinder( robot_ptr->cil6_1_R, 
		 robot_ptr->cil6_1_H, 
		 robot_ptr->cil6_1_N, 
		 robot_ptr->cil6_1_pos );
  glPopMatrix();
  glTranslatef(robot_ptr->ljoints[7], 0, 0); // end of 6th link

}

void draw_small_gripper(void)
{ 
  glPushMatrix();
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, brown);
  draw_cilinder(smallgrip_ptr->cil_R, 
		smallgrip_ptr->cil_H, 
		smallgrip_ptr->cil_N,
		smallgrip_ptr->gripper_base_pose);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, grey);
  draw_box( smallgrip_ptr->gripper_1, smallgrip_ptr->gripper_1_pose);
  glTranslatef( smallgrip_ptr->open / 2 +  5 , 0, 0);
  draw_box( smallgrip_ptr->gripper_2, smallgrip_ptr->gripper_2_pose);
  glTranslatef( - smallgrip_ptr->open - 5 , 0, 0);
  draw_box( smallgrip_ptr->gripper_2, smallgrip_ptr->gripper_2_pose);
  glPopMatrix();
}

void draw_big_gripper(void)
{ 
  glPushMatrix();
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, brown);
  draw_cilinder(biggrip_ptr->cil_R, 
		biggrip_ptr->cil_H, 
		biggrip_ptr->cil_N,
		biggrip_ptr->gripper_base_pose);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, grey);
  draw_box( biggrip_ptr->gripper_1, biggrip_ptr->gripper_1_pose );
  glTranslatef( biggrip_ptr->open / 2 +  5 , 0, 0);
  glTranslatef( 0, 50, 0);
  draw_box( biggrip_ptr->gripper_2, biggrip_ptr->gripper_2_pose );
  glTranslatef( 0, -100, 0);
  draw_box( biggrip_ptr->gripper_2, biggrip_ptr->gripper_2_pose );
  glTranslatef( - biggrip_ptr->open - 5 , 0, 0);
  draw_box( biggrip_ptr->gripper_2, biggrip_ptr->gripper_2_pose );
  glTranslatef( 0, 100, 0);
  draw_box( biggrip_ptr->gripper_2, biggrip_ptr->gripper_2_pose );
  glPopMatrix();
}

void draw_vacuum_gripper(void)
{ 
  int k;
  glPushMatrix();
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, brown);
  draw_cilinder(vgrip_ptr->cil_1_R, 
		vgrip_ptr->cil_1_H, 
		vgrip_ptr->cil_1_N,
		vgrip_ptr->gripper_base_pose);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, grey);
  draw_box( vgrip_ptr->gripper_1, vgrip_ptr->gripper_1_pose );
  glRotatef( 90.0, 0, 0, 1);
  draw_box( vgrip_ptr->gripper_1, vgrip_ptr->gripper_1_pose );
  for( k = 0; k < 4; k++ ) {
    draw_cilinder( vgrip_ptr->cil_2_R,
		   vgrip_ptr->cil_2_H,
		   vgrip_ptr->cil_2_N,
		   vgrip_ptr->cil_2_pose);
    glRotatef( 90.0, 0, 0, 1);
  }
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, yellow);
  for( k = 0; k < 4; k++ ) {
    draw_cilinder( vgrip_ptr->cil_3_R,
		   vgrip_ptr->cil_3_H,
		   vgrip_ptr->cil_3_N,
		   vgrip_ptr->cil_3_pose);
    glRotatef( 90.0, 0, 0, 1);
  }

  glPopMatrix();
}

void draw_objects(void)
{
  // if object attached - drawing it just after robot
  // else - in global frame
  
  list<Object>::iterator oit = env_ptr->object_list.begin(); 
  for( ; oit != env_ptr->object_list.end(); oit++) {
    glPushMatrix();
    if( oit->attached ){
      glLoadMatrixf( env_ptr->Mr);
      glMultMatrixf( oit->Tro );
    } else {
      glMultMatrixf( oit->Tgo );
    }
    glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, oit->color );
    draw_box( oit->box , oit->box_pos);
    glPopMatrix();
  }

}


void init(void) 
{ 

  GLfloat diffuse_light[] = { 1.0, 1.0, 1.0, 1.0 };
  GLfloat ambient_light[] = { 0.3, 0.3, 0.3, 1.0 };
  GLfloat specular_light[] = { 1.0, 1.0, 1.0, 1.0 };
  
  GLfloat mat_specular[]  = { 0.8, 0.8, 0.8, 1.0 };
  GLfloat mat_shininess[] = { 100.0 };

  glClearColor ( cellbackground[0], 
		 cellbackground[1], 
		 cellbackground[2], 
		 1.0);
  glShadeModel (GL_FLAT);
  glPolygonMode( GL_BACK, GL_POINT);
  //glEnable( GL_LINE_SMOOTH);
  glEnable(GL_DEPTH_TEST); 
  glDisable(GL_DITHER);

  glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
  glMaterialfv(GL_FRONT, GL_SPECULAR,  mat_specular);

  glLightfv( GL_LIGHT0, GL_DIFFUSE,  diffuse_light );
  glLightfv( GL_LIGHT0, GL_SPECULAR, specular_light );
  glLightfv( GL_LIGHT0, GL_AMBIENT,  ambient_light);

  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
}


void display(void)
{

  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glPushMatrix();
  glScalef(scale,scale,scale);
  glTranslatef( 0.0, shift_y, 0.0);
  glTranslatef( shift_x, 0.0, 0.0);
  gluLookAt( 2000, 2000, 2000, 100.0, 100.0, 100.0, -1.0, -1.0, 1.0);
  glRotatef(view_y, -1.0, 1.0, 0.0);
  glRotatef(view_z, 0.0, 0.0, 1.0);


  glLightfv( GL_LIGHT0, GL_POSITION, light_position );

  /* drawing coordinate axes */
  glNormal3f(-1.0,-1.0,-1.0);
  glBegin(GL_LINES);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, red);
  glVertex3f( 0.0, 0.0, 0.0);
  glVertex3f( 2000.0, 0.0, 0.0);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, yellow);
  glVertex3f( 0.0, 0.0, 0.0);  
  glVertex3f( 0.0, 2000.0, 0.0);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, blue);
  glVertex3f( 0.0, 0.0, 0.0);  
  glVertex3f( 0.0, 0.0, 1000.0);
  glEnd();

  /*  floor */
  glNormal3f(-1.0,-1.0,-1.0);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, cellfloor);
  glBegin(GL_POLYGON);
  glVertex3f( -500.0, -500.0, -2); 
  glVertex3f( 2300.0, -500.0, -2); 
  glVertex3f( 2300.0, 2300.0, -2); 
  glVertex3f( -500.0, 2300.0, -2); 
  glEnd();


  /* slide */

  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, light_green);
  draw_box(  env_ptr->slide_base,  env_ptr->slide_base_pos);
  glPushMatrix();
  glTranslatef( 0.0, 500.0, 0.0);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, light_blue);
  draw_box(  env_ptr->slide_side,  env_ptr->slide_side_pos);
  glTranslatef( 0.0, 1000.0, 0.0);
  draw_box(  env_ptr->slide_side,  env_ptr->slide_side_pos);
  glTranslatef( 485.0, -500.0, 0.0);
  glRotatef( 90.0, 0.0, 0.0, 1.0);
  draw_box(  env_ptr->slide_side,  env_ptr->slide_side_pos);
  glPopMatrix();
  glPushMatrix();
  glTranslatef( -500.0, 0.0, 0.0);
  glRotatef( 35.0, 0.0, 1.0, 0.0);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, light_green);
  draw_box( env_ptr->slide_back,  env_ptr->slide_back_pos);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, light_blue);
  glTranslatef( 0.0, -50.0, 0.0);
  draw_box( env_ptr->slide_back_side, env_ptr->slide_back_side_pos );
  glTranslatef( 0.0, 1000.0, 0.0);
  draw_box( env_ptr->slide_back_side, env_ptr->slide_back_side_pos );
  glPopMatrix();

  // tool support
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, brown);
  draw_box( env_ptr->sup_base, env_ptr->sup_base_pos);
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, grey);
  draw_cilinder( env_ptr->sup_1_R, 
		 env_ptr->sup_1_H, 
		 env_ptr->sup_1_N, 
		 env_ptr->sup_1_pos );
  draw_cilinder( env_ptr->sup_2_R, 
		 env_ptr->sup_2_H, 
		 env_ptr->sup_2_N, 
		 env_ptr->sup_2_pos );
  draw_cilinder( env_ptr->sup_3_R, 
		 env_ptr->sup_3_H, 
		 env_ptr->sup_3_N, 
		 env_ptr->sup_3_pos );
  glMaterialfv(GL_FRONT,  GL_AMBIENT_AND_DIFFUSE, darkgrey );
  draw_box( env_ptr->sup_1_1, env_ptr->sup_1_1_pos);
  draw_box( env_ptr->sup_2_1, env_ptr->sup_2_1_pos);

  switch ( robot_ptr->gripper )
    {
    case SMALL_GRIPPER :
      smallgrip_ptr->open = robot_ptr->joints[7];
      glPushMatrix();
      glTranslatef( 1000, 350, 650);
      glRotatef(180.0, 0.0, 1.0, 0.0);
      draw_big_gripper();
      glTranslatef( 0, -450, 0);
      draw_vacuum_gripper();
      glPopMatrix();
      glPushMatrix();
      draw_robot();
      glGetFloatv( GL_MODELVIEW_MATRIX, env_ptr->Mr);
      glRotatef(90.0, 0.0, 1.0, 0.0);
      draw_small_gripper();
      break;
    case BIG_GRIPPER :
      biggrip_ptr->open = robot_ptr->joints[7];
      glPushMatrix();
      glTranslatef( 1000, 150, 650);
      glRotatef(180.0, 0.0, 1.0, 0.0);
      draw_small_gripper();
      glTranslatef( 0, -250, 0);
      draw_vacuum_gripper();
      glPopMatrix();
      glPushMatrix();
      draw_robot();
      glGetFloatv( GL_MODELVIEW_MATRIX, env_ptr->Mr);
      glRotatef(90.0, 0.0, 1.0, 0.0);
      draw_big_gripper();
      break;
    case VACUUM_GRIPPER :
      glPushMatrix();
      glTranslatef( 1000, 350, 650);
      glRotatef(180.0, 0.0, 1.0, 0.0);
      draw_big_gripper();
      glTranslatef( 0, -200, 0);
      draw_small_gripper();
      glPopMatrix();
      glPushMatrix();
      draw_robot();
      glGetFloatv( GL_MODELVIEW_MATRIX, env_ptr->Mr);
      glRotatef(90.0, 0.0, 1.0, 0.0);
      draw_vacuum_gripper();
      break;
    default :
      glPushMatrix();
      glTranslatef( 1000, 150, 650);
      glRotatef(180.0, 0.0, 1.0, 0.0);
      draw_small_gripper();
      glTranslatef( 0, 200, 0);
      draw_big_gripper();
      glTranslatef( 0, -450, 0);
      draw_vacuum_gripper();
      glPopMatrix();
      glPushMatrix();
      draw_robot();
      glGetFloatv( GL_MODELVIEW_MATRIX, env_ptr->Mr);
    }
  glPopMatrix();

  // draw sample object
  if( env_ptr->show_objects ) draw_objects();

  glPopMatrix();

  glutSwapBuffers();
}

void reshape (int w, int h)
{
  glViewport (0, 0, (GLsizei) w, (GLsizei) h); 

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity (); 
  glOrtho(-2000.0 * w / h, 2000.0 * w / h , -2000.0, 2000.0, 200.0, 15000.0);
  //gluPerspective(65.0, (GLfloat) w/(GLfloat) h, 2000.0, 8000.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  
}

void cannot_move(void){
  
  glClearColor( 1.0, 0.0, 0.0, 1.0);
  display();
  glClearColor( cellbackground[0],
		cellbackground[1],
		cellbackground[2], 1.0);
  display();
}


void mouse(int button, int state, int x, int y)
{
  
  if( view_mode != CHANGE_COLOR ) {

    switch( button ) {
    case GLUT_LEFT_BUTTON:
      view_mode = ROTATE_VIEW;
      break;
    case GLUT_RIGHT_BUTTON:
      view_mode = TRANSLATE_VIEW;
      break;
    case GLUT_MIDDLE_BUTTON:
      view_mode = SCALE_VIEW;
      break;
    default:
      break;
    }

  }

  cursor_x = x;
  cursor_y = y;

  return;
}

void keyboard (unsigned char key, int x, int y)
{
  char strkey[2] = " "; 

  switch ( oldkey ) {
  case 't' :
    strkey[0] = key;
    robot_ptr->attach_object( atoi(strkey) ); 
    glutPostRedisplay();
    oldkey = key;
    return;
    break;
  case 'r' :
    strkey[0] = key;
    robot_ptr->detach_object( atoi(strkey) );
    glutPostRedisplay();
    oldkey = key;
    return;
    break;
  case 'g' :
    strkey[0] = key;
    robot_ptr->attach_gripper( atoi(strkey) );
    glutPostRedisplay();
    oldkey = key;
    return;
    break;
  case 'c' :
    if( key == 'b' ) colormode = BACKGROUND_COLOR;  
    if( key == 'f' ) colormode = FLOOR_COLOR;
    view_mode = CHANGE_COLOR;
    current_joint = 0;
    oldkey = key;
    return;
    break;
  default:
    oldkey = key;
    break;
  }

  switch (key) {
  case '0':
    current_joint = 0;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case '1':
    current_joint = 1;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case '2':
    current_joint = 2;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case '3':
    current_joint = 3;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case '4':
    current_joint = 4;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case '5':
    current_joint = 5;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case '6':
    current_joint = 6;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case '7':
    current_joint = 7;
    joints_mode = JOINT;
    glutPostRedisplay();
    break;
  case 'h':
    robot_ptr->joints[1] =  
      robot_ptr->joints[2] =
      robot_ptr->joints[3] =
      robot_ptr->joints[4] =
      robot_ptr->joints[5] =
      robot_ptr->joints[6] = 0;
    robot_ptr->joints[7] = 200;   
    glutPostRedisplay();
    break;
  case 'V':
    view_z = 10;  
    view_y = 0;
    shift_x = -300;  
    shift_y = 0;
    scale = 1.0;
    glutPostRedisplay();
    break;
  case 'v':
    current_joint = 0;
    view_mode = ROTATE_VIEW;
    break;
  case 'w':
    glutReshapeWindow(500, 500); 
    glutPostRedisplay();
    break;
  case 'W':
    glutFullScreen();
    glutPostRedisplay();
    break;
  case 'c':
    key = 'c';
    break;
  case 'R':
    colortype = 0;
    break;
  case 'G':
    colortype = 1;
    break;
  case 'B':
    colortype = 2;
    break;
  case 'x':
    joints_mode = CART_MOVE_X;
    current_joint = 1;
    break;  
  case 'y':
    joints_mode = CART_MOVE_Y;
    current_joint = 1;
    break;  
  case 'z':
    joints_mode = CART_MOVE_Z;
    current_joint = 1;
    break;
  case 'X':
    joints_mode = CART_ROTATE_X;
    current_joint = 1;
    break;  
  case 'Y':
    joints_mode = CART_ROTATE_Y;
    current_joint = 1;
    break;  
  case 'Z':
    joints_mode = CART_ROTATE_Z;
    current_joint = 1;
    break;
  case 'j':
    joints_mode = JOINT;
    current_joint = 1;
    break;
  case 'F':
    robot_ptr->set_net_timeout(100);
    robot_ptr->set_cpu_idle(0);
    break;
  case 'f':
    robot_ptr->set_net_timeout(100000);
    robot_ptr->set_cpu_idle(100000);
    break;
  case 't':
    key = 't'; 
    break;
  case 'r':
    key = 'r';
    break;
  case 'd':
    robot_ptr->detach_gripper();
    break;
  case 'g':
    key = 'g';
    glutPostRedisplay();
    break;
  case 'm':
    viewtable++;
    viewtable = viewtable%2;
    glutPostRedisplay();
    break;
  case '+':
    scale *= 1.02; 
    glutPostRedisplay();
    break;
  case '-':
    scale *= 0.98; 
    glutPostRedisplay();
    break;
  case 'q':
    msg_ptr->~Message();
    exit(0);
    break;    

  case 27:
    msg_ptr->~Message();
    exit(0);
    break;
  default:
    break;
  }
  oldkey = key;
}

void motion(int x, int y)
{
  int delta = 0;
  if( current_joint != 0 ) {

    if( abs(cursor_x - x) > 30) cursor_x = x; 
    delta = x - cursor_x;
    
    if( joints_mode == JOINT ){  // JOINT MOVEMENT
      float f = delta;
      if(current_joint != 7 ) { 
	f *= 0.01;
      }
      if( ( ( robot_ptr->joints[current_joint] + f ) 
	    <= robot_ptr->max[current_joint] ) && 
	  ( ( robot_ptr->joints[current_joint] + f) 
	  >= robot_ptr->min[current_joint] ) ) {
	robot_ptr->joints[current_joint] += f;
      } else {
	cannot_move();
	cerr << "limit ! " << robot_ptr->joints[current_joint] 
	     << " : "
	     <<  robot_ptr->min[current_joint] << " - " 
	     << robot_ptr->max[current_joint] << endl;
      }
    } else {   // CARTHESIAN MOVEMENTS 

      // here goes reverse kinematic 

      GL_Matrix frame = Get_Robot_Frame(robot_ptr->joints);

      switch( joints_mode ){
      case CART_MOVE_X:
	frame(0,3) += delta * 5;
	break;
      case CART_MOVE_Y:
	frame(1,3) += delta * 5;
	break;
      case CART_MOVE_Z: 
	frame(2,3) += delta * 5;
	break;
      case CART_ROTATE_X:
	{
	  double ca = cos( 0.01 * delta );
	  double sa = sin( 0.01 * delta );
	  double x = frame(0,3);
	  double y = frame(1,3);
	  double z = frame(2,3);
	  frame(0,3) = frame(1,3) = frame(2,3) = 0;

	  GL_Matrix R( 1,   0,   0,  0,
		       0,  ca, -sa,  0,
		       0,  sa,  ca,  0 );
	  frame = R * frame;

	  frame(0,3) = x;
	  frame(1,3) = y;
	  frame(2,3) = z;
	}
	break;
      case CART_ROTATE_Y:
	{
	  double ca = cos( 0.01 * delta );
	  double sa = sin( 0.01 * delta );
	  double x = frame(0,3);
	  double y = frame(1,3);
	  double z = frame(2,3);

	  frame(0,3) = frame(1,3) = frame(2,3) = 0;

	  GL_Matrix R(  ca,  0,  sa, 0,
			0,   1,   0, 0,
			-sa, 0,  ca, 0 );
	  frame = R * frame;

	  frame(0,3) = x;
	  frame(1,3) = y;
	  frame(2,3) = z;
	}
	break;
      case CART_ROTATE_Z:
	{
	  double ca = cos( 0.01 * delta );
	  double sa = sin( 0.01 * delta );
	  double x = frame(0,3);
	  double y = frame(1,3);
	  double z = frame(2,3);

	  frame(0,3) = frame(1,3) = frame(2,3) = 0;

	  GL_Matrix R( ca, -sa,  0,  0,
		       sa,  ca,  0,  0,
		       0,    0,  1,  0 );
	  frame = R * frame;

	  frame(0,3) = x;
	  frame(1,3) = y;
	  frame(2,3) = z;
	}
	break;
      default:
	break;
      }
      double joints[7];
      if( Frame_To_Joints( frame, joints) ){
	if( (joints[1] <= robot_ptr->max[1]) &&
	    (joints[2] <= robot_ptr->max[2]) &&
	    (joints[3] <= robot_ptr->max[3]) &&
	    (joints[4] <= robot_ptr->max[4]) &&
	    (joints[5] <= robot_ptr->max[5]) &&
	    (joints[6] <= robot_ptr->max[6]) &&
	    (joints[1] >= robot_ptr->min[1]) &&
	    (joints[2] >= robot_ptr->min[2]) &&
	    (joints[3] >= robot_ptr->min[3]) &&
	    (joints[4] >= robot_ptr->min[4]) &&
	    (joints[5] >= robot_ptr->min[5]) &&
	    (joints[6] >= robot_ptr->min[6]) ) {
	  for(int j = 0; j < 7; j++) robot_ptr->joints[j]=joints[j];
	} else {
	  cannot_move();
	  cerr << "limit ! " << joints[1] << " " 
	       << joints[2] << " " 
	       << joints[3] << " " 
	       << joints[4] << " " 
	       << joints[5] << " " 
	       << joints[6] << endl;
	}
      }
    }
    cursor_x = x;

  } else {    // Viewing
    if( abs(cursor_x - x) > 30) cursor_x = x;
    delta = (cursor_x - x ) * MY_GL_ACSELL;
    if(view_mode == ROTATE_VIEW){
      view_z -= delta;
    } else if( view_mode == TRANSLATE_VIEW ) {
      shift_x -= ( delta * 20);
    } else if( view_mode == CHANGE_COLOR ) {
      if( colormode == FLOOR_COLOR ) {
	cellfloor[colortype] -= (delta * 0.001);
	if( cellfloor[colortype] > 1 ) cellfloor[colortype] = 1;
	if( cellfloor[colortype] < 0 ) cellfloor[colortype] = 0;
      } else {
	cellbackground[colortype] -= (delta * 0.001);
	if( cellbackground[colortype] > 1 ) 
	  cellbackground[colortype] = 1;
	if( cellbackground[colortype] < 0 ) 
	  cellbackground[colortype] = 0;
	glClearColor( cellbackground[0], 
		      cellbackground[1], 
		      cellbackground[2], 
		      1.0 );
      }
    }
    cursor_x = x;

    if( abs(cursor_y - y) > 30) cursor_y = y;
    delta = (cursor_y - y) * MY_GL_ACSELL;
    if( view_mode == ROTATE_VIEW ){ 
      view_y -= delta;
    } else if( view_mode == TRANSLATE_VIEW ) {
      shift_y += ( delta * 20);
    } else if( view_mode == SCALE_VIEW ){
      scale -= ( delta * 0.001);
    }
    cursor_y = y;
  }
  glutPostRedisplay();

}

void gl_get_task(void)
{
  robot_ptr->get_task();
  glutPostRedisplay();
}

int gl_server_init(void)
{
  int c = 1;
  char* v[] = { "robot_server" };
  glutInit( &c, v);
  glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowSize (500, 500); 
  glutInitWindowPosition (100, 100);
  glutCreateWindow ("Robot server 1.0");
  init ();
  glutDisplayFunc(display); 
  glutReshapeFunc(reshape);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
  glutKeyboardFunc(keyboard);
  glutIdleFunc( gl_get_task );

  return 0;
}



int main( int argc, char* argv[])
{

  Small_Gripper smallgripper;
  smallgrip_ptr = &smallgripper;
  Big_Gripper biggripper;
  biggrip_ptr = &biggripper;
  Vacuum_Gripper vacuumgripper;
  vgrip_ptr = &vacuumgripper;
  Environment env;
  env_ptr = &env;
  Message msg( atoi(argv[1]) );
  msg_ptr = &msg;
  GL_Robot Robot( msg_ptr, smallgrip_ptr, biggrip_ptr, env_ptr );
  robot_ptr = &Robot;

  gl_server_init();

  glutMainLoop();
  return 0;
}
