#include <iostream.h>
#include <unistd.h>
#include <stdlib.h> 
#include <math.h>
#include <time.h>
#include <sys/times.h> 
#include <GL/glut.h>
#include "GL_Robot.h"
#include "GL_Common.h"
#include <Command_Type.h>
#include "Environment.h"
#include "Small_Gripper_Structure.h"
#include "Big_Gripper_Structure.h"
#include "Calculations.h"


// class GL_Robot and it's methods


GL_Robot::GL_Robot( Message *msg_ptr, 
		    Small_Gripper* sg,
		    Big_Gripper* bg,
		    Environment* env) {
  
  robot_server = msg_ptr;

  joints[0] = joints[1] = joints[2] = joints[3] 
    = joints[4] = joints[5] = joints[6] = 0.0; 
  joints[7] = 0.0;
  ljoints[0] = 675.0;
  ljoints[1] = 300.0;
  ljoints[2] = 650.0;
  ljoints[3] = 155.0;
  ljoints[4] = 300.0;
  ljoints[5] = 300.0;
  ljoints[6] = 100.0;
  ljoints[7] = 40.0;

  // min-max of values of robot joints
  min[1] = -3.15 ;  max[1] = 3.15 ;
  min[2] = -1.58 ;  max[2] = 1.58 ;
  min[3] = -1.58 ;  max[3] = 1.58 ;
  min[4] = -3.15 ;  max[4] = 3.15 ;
  min[5] = -1.58 ;  max[5] = 1.58 ;
  min[6] = -3.15 ;  max[6] = 3.15 ;
  min[7] = 0.0 ;    max[7] = 0.0 ;


  // SPEED OF ROBOT 
  js[1] = 0.4;
  js[2] = 0.4;
  js[3] = 0.4;
  js[4] = 0.5;
  js[5] = 0.5;
  js[6] = 0.6;
  js[7] = 0.0;

  net_timeout = 1000;
  gl_time = 0; // 1  for correcting pose at the end of movement 
  rt_var = 20;
  cpu_idle = 0; // gives 0 mcsec to system for other tasks, #:0) 

  gripper = 0; // no gripper attached
  env_ptr = env;
  sgrip_ptr = sg;
  bgrip_ptr = bg;
}

GL_Robot::~GL_Robot(void){
;
}


int GL_Robot::attach_gripper(int griptype) {
    
  switch( griptype ){
  case BIG_GRIPPER:
    max[7] = bgrip_ptr->max;
    min[7] = bgrip_ptr->min;
    joints[7] = bgrip_ptr->open;
    break;
  case SMALL_GRIPPER:
    max[7] = sgrip_ptr->max;
    min[7] = sgrip_ptr->min;
    joints[7] = sgrip_ptr->open;
    break;
  case VACUUM_GRIPPER:
    max[7] = min[7] = 0;
    joints[7] = 0;
    break;
  }
  gripper = griptype; 
  return 0; 
}


int GL_Robot::detach_gripper(void) {
  
  joints[7] = 0;
  js[7] = 0;
  max[7] = 0;
  min[7] = 0;

  gripper = NONE; 
  return 0; 
}

int GL_Robot::set_joints(double* param){ 

  // necessary to decrease amount of CPU usage
  if(cpu_idle != 0 ) usleep(cpu_idle);
  
  joints[1] =  param[0];
  joints[2] =  param[1];
  joints[3] =  param[2];
  joints[4] =  param[3];
  joints[5] =  param[4];
  joints[6] =  param[5];
  if( (gripper != NONE) && (gripper != VACUUM_GRIPPER) ) { 
    joints[7] =  param[6];
  }
  return 0;
}

int GL_Robot::move_joints_by(double* param){ 

  if( ( ( param[0] + joints[1] ) <= max[1] ) && 
      ( ( param[0] + joints[1] ) >= min[1] ) &&
      ( ( param[1] + joints[2] ) <= max[2] ) && 
      ( ( param[1] + joints[2] ) >= min[2] ) &&
      ( ( param[2] + joints[3] ) <= max[3] ) && 
      ( ( param[2] + joints[3] ) >= min[3] ) &&
      ( ( param[3] + joints[4] ) <= max[4] ) && 
      ( ( param[3] + joints[4] ) >= min[4] ) &&
      ( ( param[4] + joints[5] ) <= max[5] ) && 
      ( ( param[4] + joints[5] ) >= min[5] ) &&
      ( ( param[5] + joints[6] ) <= max[6] ) && 
      ( ( param[5] + joints[6] ) >= min[6] ) &&
      ( ( param[6] + joints[7] ) <= max[7] ) && 
      ( ( param[6] + joints[7] ) >= min[7] ) ) {
    
    tms tmsbuffer;
    real_time = times(&tmsbuffer);
    double t[7]; 

    t[0] = param[0] / js[1];
    t[1] = param[1] / js[2];
    t[2] = param[2] / js[3];
    t[3] = param[3] / js[4];
    t[4] = param[4] / js[5];
    t[5] = param[5] / js[6];
    if( js[7] != 0 ) {
      t[6] = param[6] / js[7];
    } else {
      t[6] = 0;
    }

    int i;
    Tmax = sqrt( t[6] * t[6] ) ;
    for( i = 0; i < 6; i++){
      t[i] = sqrt( t[i] * t[i]);
      if( Tmax < t[i]) Tmax = t[i];
    }
    cerr << endl << endl << "max time : " <<  Tmax << endl;

    gl_time = (int) rint( Tmax * rt_var );  
    cerr << "max gl time : " << gl_time  << endl;
    if( gl_time ) { 
      for(i = 0; i < 7; i++) {
	delta[i] =  param[i] / gl_time ;
	dest_pose[i] = joints[i+1] + param[i];
      }
    } else {
      for(i = 0; i < 7; i++) {
	//delta[i] =  param[i];
	dest_pose[i] = joints[i+1] + param[i];
      }
      gl_time = 1; // correcting pose at least
    }
    

    return 0;
  }
  cerr << "limits !!!" << endl;
  int jc;
  for( jc = 0; jc < 7; jc++ ) cerr << " " << param[jc];
  cerr << endl;
  for( jc = 1; jc < 8; jc++ ) cerr << " " << joints[jc];
  cannot_move();
  return -1;
}

int GL_Robot::move_joints_to(double* param){
  for(int i = 0; i < 7; i++){
    param[i] -= joints[i+1]; 
  }
  move_joints_by(param);

  return 0;
}

int GL_Robot::move_joint_by(long num, double param){

  double par[7] = { 0,0,0,0,0,0,0 };

  if( num > 8 ) return -1;

  par[ num - 1 ] = param; 
  move_joints_by(par);

  return 0;
}

int GL_Robot::move_joint_to(long num, double param){

  double par[7] = { 0,0,0,0,0,0,0 };

  if( num > 8 ) return -1;

  par[num - 1] = param - joints[num]; 
  move_joints_by(par);

  return 0;
}


int GL_Robot::move_rpy_to(double* param){

  double js[7],jsdif[7];  
  RPY_To_Joints( param, js);
  for(int i = 0; i < 6; i++ ) {
    jsdif[i] = js[i+1] - joints[i+1];
  }
  jsdif[6] = 0;
  move_joints_by( jsdif);

  return 0;
}


int GL_Robot::move_rpy_by(double* param){

  double js[7], jsdif[7], rpy[6];
  int i;

  //for( int k = 0; k < 6; k++ ) cerr << param[k] << endl;

  GL_Matrix m = Get_Robot_Frame(joints);
  Frame_To_RPY( m, rpy);

  for( i = 0; i < 6; i++ ) {
    rpy[i] += param[i];
  }

  if( RPY_To_Joints( rpy, js) ){
    
    for( i = 0; i < 6; i++ ) {
      jsdif[i] = js[i+1] - joints[i+1];
    }
    jsdif[6] = 0;
    move_joints_by( jsdif);
    return 0;
  }
  
  return -1;
}


int GL_Robot::linear_move_by(double* param){

  


  return 0;
}


int GL_Robot::linear_move_to(double* param){

  


  return 0;
}


int GL_Robot::redisplay(void){

  double set_joints_param[7];

  if( gl_time < 2 ) {
    //correcting pose
    set_joints(dest_pose);
    //for( int dj = 0; dj < 7; dj++) cerr << dest_pose[dj] << endl;

    // real_time estimation
    if( Tmax ) {

      tms tmsbuffer;
      real_time = ( times(&tmsbuffer) - real_time ) / CLK_TCK ;
      cerr << "real time :" << real_time << endl;
      if( real_time ) {
	int rt_var_tmp = (int) ceil( Tmax / real_time * rt_var);
	if( rt_var_tmp > 3 )  rt_var = rt_var_tmp;
      }  
    }
    cerr << "rt_var :" << rt_var << endl;  

  } else {

    set_joints_param[0] = delta[0] + joints[1];
    set_joints_param[1] = delta[1] + joints[2];
    set_joints_param[2] = delta[2] + joints[3];
    set_joints_param[3] = delta[3] + joints[4];
    set_joints_param[4] = delta[4] + joints[5];
    set_joints_param[5] = delta[5] + joints[6];
    set_joints_param[6] = delta[6] + joints[7];
    set_joints(set_joints_param);
  }

  gl_time--;

  return 0;
}

int GL_Robot::set_net_timeout(long t){

  net_timeout = t;
  cerr << "net_timeout(mcsec) : " << net_timeout << endl;

  return 0;
}

int GL_Robot::set_cpu_idle(long t){

  cpu_idle = t;
  cerr << "cpu idle(mcsec) : " << cpu_idle << endl;

  return 0;
}

int GL_Robot::set_limits(double* param){
  
  int i,j;

  for( i = 0, j = 1; i < 12; i += 2, j++ ){
    min[j] = param[i]; 
    max[j] = param[i+1];
  }
  bgrip_ptr->min = param[i++];
  bgrip_ptr->max = param[i++];
  sgrip_ptr->min = param[i++];
  sgrip_ptr->max = param[i++];

  return 0;
}

int GL_Robot::set_speeds(double* param){

  if( param[0] && param[1] && param[2] && 
      param[3] && param[4] && param[5] ) {

    js[1] = param[0];
    js[2] = param[1];
    js[3] = param[2];
    js[4] = param[3];
    js[5] = param[4];
    js[6] = param[5];

    bgrip_ptr->speed = param[6];
    sgrip_ptr->speed = param[7];
    if( gripper == SMALL_GRIPPER ) { js[7] = sgrip_ptr->speed; }
    if( gripper == BIG_GRIPPER ) { js[7] = bgrip_ptr->speed; }
    
  }
  return 0;
}

int GL_Robot::attach_object(int num)
{ 
  int i,j;

  if( (unsigned int) num >= env_ptr->object_list.size() ) return -1;
  
  GL_Matrix TR = Get_Robot_Frame( joints );
 
  list<Object>::iterator oit = env_ptr->object_list.begin();
  for( ; num != 0; oit++, num--);
  if( oit->attached == true ) return -1;

  GL_Matrix TO;
  for(i = 0; i < 4; i++)
    for(j = 0; j < 4; j++)
      TO(j,i) =  oit->Tgo[i * 4 + j];

  GL_Matrix TRO;
  TRO = !TR * TO;

  for(i = 0; i < 4; i++)
    for(j = 0; j < 4; j++)
      oit->Tro[i * 4 + j] = TRO(j,i);

  oit->attached = true;

  return 0;
}

int GL_Robot::detach_object( int num )
{ 
  int i,j;

  if( (unsigned int) num >= env_ptr->object_list.size() ) return -1;

  GL_Matrix TR = Get_Robot_Frame( joints );
  
  list<Object>::iterator oit = env_ptr->object_list.begin();
  for( ; num != 0; oit++, num--);

  if( oit->attached == false ) return -1;

  GL_Matrix TRO;
  for(i = 0; i < 4; i++)
    for(j = 0; j < 4; j++)
      TRO(j,i) = oit->Tro[i * 4 + j];

  GL_Matrix TO = 
    Get_Robot_Frame( joints ) * TRO;
  for(i = 0; i < 4; i++)
    for(j = 0; j < 4; j++)
      oit->Tgo[i * 4 + j] = TO(j,i);

  oit->attached = false;

  return 0;
}


int GL_Robot::get_task(void) {

  if( gl_time < 1 ){ 
    
    My_Message com;
    if(robot_server->receive( com, 1) != -1) {
      //cerr << com.command << endl;
      //cerr << com.lparam << endl;
      //for(int k = 0; k < 20; k++ ) cerr << com.param[k] << endl;

      switch( com.command ) 
	{
	case STOP_SERVER:
	  cerr << "stop " << endl;
	  break;
	case MOVE_JOINTS_TO: 
	  move_joints_to( com.param );
	  break;  
	case MOVE_JOINTS_BY: 
	  move_joints_by( com.param );
	  break; 
	case MOVE_JOINT_BY: 
	  move_joint_by( com.lparam, com.param[com.lparam-1] );
	  break; 
	case MOVE_JOINT_TO: 
	  move_joint_to( com.lparam, com.param[com.lparam-1] );
	  break;
	case MOVE_RPY_BY: 
	  move_rpy_by( com.param );
	  break; 
	case MOVE_RPY_TO: 
	  move_rpy_to( com.param );
	  break; 
	case LINEAR_MOVE_TO: 
	  linear_move_to( com.param );
	  break; 
	case LINEAR_MOVE_BY: 
	  linear_move_by( com.param );
	  break;
	case SET_LIMITS: 
	  set_limits( com.param );
	  break; 
	case SET_SPEEDS: 
	  set_speeds( com.param );
	  break;  
	case ATTACH_SMALL_GRIPPER: 
	  joints[7] = sgrip_ptr->open;
	  attach_gripper( SMALL_GRIPPER );
	  max[7] =  sgrip_ptr->max;
	  min[7] =  sgrip_ptr->min;
	  js[7] =  sgrip_ptr->speed;
	  break;  
	case DETACH_SMALL_GRIPPER: 
	  detach_gripper();
	  break; 
	case ATTACH_BIG_GRIPPER: 
	  joints[7] = bgrip_ptr->open;
	  attach_gripper( BIG_GRIPPER );
	  max[7] = bgrip_ptr->max;
	  min[7] = bgrip_ptr->min;
	  js[7] = bgrip_ptr->speed;
	  break;  
	case DETACH_BIG_GRIPPER: 
	  detach_gripper();
	  break;
	case ATTACH_VACUUM_GRIPPER: 
	  joints[7] = 0;
	  attach_gripper( VACUUM_GRIPPER );
	  max[7] = 0.0;
	  min[7] = 0.0;
	  js[7] = 0.0;
	  break;  
	case DETACH_VACUUM_GRIPPER: 
	  detach_gripper();
	  break;
	case GRASP_OBJECT: 
	  attach_object( com.lparam );
	  break;
	case RELEASE_OBJECT: 
	  detach_object( com.lparam );
	  break;
	case SET_NET_TIMEOUT: 
	  set_net_timeout( com.lparam );
	  break; 
	case SET_CPU_IDLE: 
	  set_cpu_idle( com.lparam );
	  break;
	case ADD_OBJECT:

	  GLfloat size[3], pose[3], frame[16], color[3]; 
	  size[0] = com.param[0];
	  size[1] = com.param[1];
	  size[2] = com.param[2];

	  pose[0] = com.param[3];
	  pose[1] = com.param[4];
	  pose[2] = com.param[5];

	  frame[0]  = com.param[6];
	  frame[4]  = com.param[7];
	  frame[8]  = com.param[8];
	  frame[12] = com.param[9];
	  frame[1]  = com.param[10];   
	  frame[5]  = com.param[11];
	  frame[9]  = com.param[12];
	  frame[13] = com.param[13];
	  frame[2]  = com.param[14];
	  frame[6]  = com.param[15];
	  frame[10] = com.param[16];
	  frame[14] = com.param[17];
	  frame[3]  = 0;
	  frame[7]  = 0;
	  frame[11] = 0;
	  frame[15] = 1;
	  
	  color[0] = rint( com.lparam / 10000 );
	  color[1] = rint( (com.lparam - color[0] * 10000) / 100 );
	  color[2] = ( com.lparam % 100 );

	  color[0] *= 0.01;
	  color[1] *= 0.01;
	  color[2] *= 0.01;

	  env_ptr->add_object( size, pose, frame, color);
	  break;
	case REMOVE_OBJECT: 
	  env_ptr->remove_object( com.lparam );
	  break;
	case GET_INFO:
	  answer_to_client( com.lparam );
	  break;
	}
    } else {
      //cerr << ".";
      usleep(net_timeout);
    }
  } else {
    redisplay();
  }

  return 0;
}

int GL_Robot::answer_to_client(int infotype) {

  My_Message com;
  int i,j;

  switch( infotype){
  case JOINTS_INFO:
    {
      com.message_type = 2;
      com.command = JOINTS_INFO;
      com.lparam = gripper;
      GL_Matrix m = Get_Robot_Frame( joints );
      double rpy[6];
      Frame_To_RPY( m, rpy);
      for( i = 0; i < 7; i++){
	com.param[i] = joints[i+1];
      }
      for( i = 7; i < 13; i++){
	com.param[i] = rpy[i - 7];
      }
      
      robot_server->send( com);
    }
    break;

  case LIMITS_INFO:
    {
      com.message_type = 2;
      com.command = LIMITS_INFO;
      for( i = 0, j = 1; i < 12; j++, i += 2 ){
	com.param[i] = min[j];
	com.param[i+1] = max[j];
      }

      com.param[i++] = bgrip_ptr->min;
      com.param[i++] = bgrip_ptr->max;
      com.param[i++] = sgrip_ptr->min;
      com.param[i++] = sgrip_ptr->max;

      com.param[i++] = cpu_idle;
      com.param[i++] = net_timeout;

      robot_server->send( com);
    }
    break;
    
  case SPEEDS_INFO:
    {
      com.message_type = 2;
      com.command = SPEEDS_INFO;
      for( i = 0; i < 6; i++){
	com.param[i] = js[i+1];
      }
      com.param[i++] = bgrip_ptr->speed;
      com.param[i++] = sgrip_ptr->speed;
      robot_server->send( com);
    }
    break;
  default:
    return -1;
  }

  return 0;
}
