#include <iostream>
#include <Robot.h>
#include <Command_Type.h>
#include <unistd.h>   
#include <string>
#include "Sample_Calls.h"




int main( int argc, char* argv[])
{
  if( argc < 3 ) {
    cerr << "not enough parameters!" << endl;
    cerr << "Call this programm as \" ./corba_client host XXXXX \","
	 << endl << "where host is address(name) of robot server," 
	 << endl << "XXXXX is a port number of that server" << endl;

    exit(1);
  }
  int tempint = strtol( argv[2], NULL, 10);
  if( tempint < 1025 || tempint > 65536 ){
    cerr << "Call this programm as \" ./corba_client host XXXXX \","
	 << endl << "where host is address(name) of robot server," 
	 << endl << "XXXXX is a port number of that server" << endl;
    exit(1);
  }
  if( strlen( argv[1]) > 68) {
    cerr << "Sorry, hostname is too long" << endl;
    exit(1);
  }

  int i, j = 1;
  int c = 3;
  char* str[3]; 
  char progname[] = "robot_client";
  char corbaparam[] = "-ORBBindAddr";
  char robotaddress[80];
  string raddress = "inet:";
  raddress += argv[1];
  raddress += ":";
  raddress += argv[2];
  strcpy(robotaddress,raddress.c_str());
  str[0] = progname;
  str[1] = corbaparam;
  str[2] = robotaddress;

  // orb initialization
  CORBA::ORB_var orb = CORBA::ORB_init( c, str, "mico-local-orb");
  CORBA::Object_var obj = orb->bind ("IDL:Robot:1.0");
  if (CORBA::is_nil (obj)) {
    cerr << "#:0(  : Bind to Robot failed" << endl;
    exit (1);
  }
  Robot_var robot = Robot::_narrow (obj);

  CORBA::Any any_command;
  Robot_Command com;


  ///////////////// query //////////////////////////////
  // done with get_info method ( don't confuse with put_command )


  // ask limits of joins and grippers

  com.command = GET_INFO;
  com.lparam =  LIMITS_INFO;

  any_command <<= com;
  while( robot->get_info(any_command) == -1 ) sleep(1);
  any_command >>=com;
  
  cerr << "joints limits, degrees: " << endl;
  for( j = 0, i = 1; j < 12; j += 2, i++) {
    cerr << "joint N" << i << " : "  
	 << "min = " << com.param[j] * 57.296 << " " 
	 << "max = " << com.param[j+1]  * 57.296 << endl;
  }
  cerr << "big gripper : " << "min = "   << com.param[j++];
  cerr << " " << "max = " << com.param[j++] << endl;
  cerr << "small gripper : " << "min = " << com.param[j++];
  cerr << " " << "max = " << com.param[j++] << endl;

  cerr << "cpu idle : " << com.param[j++] << " mcsec" << endl;
  cerr << "net timeout : " << com.param[j++] << " mcsec" << endl;

  // current state

  com.command = GET_INFO;
  com.lparam =  JOINTS_INFO;

  any_command <<= com;
  while( robot->get_info(any_command) == -1 ) sleep(1);
  any_command >>=com;
  
  cerr << endl << "joints values: " << endl;
  for( j = 0; j < 6; j++) {
    cerr << "joint N" << j + 1 << " : " << com.param[j] * 57.296 
	 << " degrees" << endl;
  }
  cerr << "gripper attached : " << com.lparam << endl;
  cerr << "opening : " << com.param[6] << endl;

  cerr << "RPY :" << endl;
  for( j = 7; j < 10; j++) {
    cerr << com.param[j] << endl;
  }
  for( j = 10; j < 13; j++) {
    cerr << com.param[j] * 57.296 << endl;
  }


  // speeds

  com.command = GET_INFO;
  com.lparam =  SPEEDS_INFO;

  any_command <<= com;
  while( robot->get_info(any_command) == -1 ) sleep(1);
  any_command >>=com;
  
  cerr << endl << "speeds values: " << endl;
  for( j = 0; j < 6; j++) {
    cerr << "joint N" << j + 1 << " : " << com.param[j] * 57.296 
	 << " degrees per second" << endl;
  }
  cerr << "big gripper : " << com.param[j++] << " mm per second" 
       << endl;
  cerr << "small gripper : " << com.param[j++] << " mm per second"
       << endl;


  ///////////// setting speeds //////////////////////////

  cerr << endl << "setting new speeds" << endl;

  com.command = SET_SPEEDS;
  com.param[0]  = 0.7;
  com.param[1]  = 0.7;
  com.param[2]  = 0.7;
  com.param[3]  = 1.5;
  com.param[4]  = 1.5;
  com.param[5]  = 1.5;

  com.param[6]  = 200; // big gripper
  com.param[7]  = 250; // small gripper

  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);


  //////// setting cpu idle and net timeout ////////////////

  cerr << endl << "setting cpu idle and net timeout" << endl;

  com.command = SET_CPU_IDLE;
  com.lparam  = 0;
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  com.command = SET_NET_TIMEOUT;
  com.lparam  = 100;
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);


  /////////// removing all old boxes ///////////////////  

  cerr << endl << "removing all old objects" << endl;

  com.command = REMOVE_OBJECT;
  com.lparam = 0;
  any_command <<= com;
  for( j = 0; j < 20; j++){
    while( robot->put_command(any_command) == -1 ) sleep(1);
  }


  ////////////  movements ///////////////////////////////

  // trying joint movements

  move_joints_to( robot, 0, 0, 0, 0, 0, 0, 0); 
 
  for( j = 0; j < 6; j++ ){
    
    move_joint_by( robot, j + 1, 0.5 );
    move_joint_by( robot, j + 1, -1 );
    move_joint_by( robot, j + 1, 0.5 );
  }


  // trying grippers

  take_big_gripper(robot);
  open_gripper(robot, 0);
  open_gripper(robot, 400);
  put_big_gripper(robot);
  take_small_gripper(robot);
  open_gripper(robot, 0);
  open_gripper(robot, 250);
  put_small_gripper(robot);
  take_vacuum_gripper(robot);
  put_vacuum_gripper(robot);

  
  // trying RPY movements

  take_big_gripper(robot);
  
  go_home(robot);
  
  open_gripper(robot,400);

  move_rpy_by( robot, 200, 0, 0, 0, 0, 0);
  move_rpy_by( robot, -400 , 0, 0, 0, 0, 0);
  move_rpy_by( robot, 200 , 0, 0, 0, 0, 0);

  move_rpy_by( robot, 0, 200, 0, 0, 0, 0);
  move_rpy_by( robot, 0, -400, 0, 0, 0, 0);
  move_rpy_by( robot, 0 , 200, 0, 0, 0, 0);

  move_rpy_by( robot, 0, 0, 200, 0, 0, 0);
  move_rpy_by( robot, 0, 0, -400, 0, 0, 0);
  move_rpy_by( robot, 0, 0, 200, 0, 0, 0);


  put_big_gripper(robot);
  go_home(robot);

  sleep(5);

  
  //........ let't make show - putting boxes on the floor

  while(1) {

    go_home(robot);

    /////////// removing all old boxes ///////////////////  

    cerr << endl << "removing all old objects" << endl;

    com.command = REMOVE_OBJECT;
    com.lparam = 0;
    any_command <<= com;
    for( j = 0; j < 20; j++){
      while( robot->put_command(any_command) == -1 ) sleep(1);
    }

    ////////// adding boxes //////////////////////////////

    cerr << endl << "Adding new boxes" << endl;

    com.command = ADD_OBJECT;
    com.lparam = 990000; // rgb color - 99% red
    // sizes, x,y,z, mm
    com.param[0]  = 200;
    com.param[1]  = 240;
    com.param[2]  = 200;
    // local sistem of coordinate, x,y,z, mm, usually in the center
    com.param[3]  = 0;
    com.param[4]  = 0;
    com.param[5]  = 0;
    // frame - coordinates of local frame in global system
    com.param[6]  = 1;
    com.param[7]  = 0;
    com.param[8]  = 0;
    com.param[9]  = 0; // x
    com.param[10] = 0;
    com.param[11] = 1;
    com.param[12] = 0;
    com.param[13] = 800; // y
    com.param[14] = 0;
    com.param[15] = 0;
    com.param[16] = 1;
    com.param[17] = 150; // z
  
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);


    com.lparam = 888800; // rgb color - 88% red, 88% green = yellow
    // sizes, x,y,z, mm
    com.param[0]  = 200;
    com.param[1]  = 240;
    com.param[2]  = 200;
    // local sistem of coordinate, x,y,z, mm, usually in the center
    com.param[3]  = 0;
    com.param[4]  = 0;
    com.param[5]  = 0;
    // frame - coordinates of local frame in global system
    com.param[6]  = 1;
    com.param[7]  = 0;
    com.param[8]  = 0;
    com.param[9]  = 0; // x
    com.param[10] = 0;
    com.param[11] = 1;
    com.param[12] = 0;
    com.param[13] = 1200; // y
    com.param[14] = 0;
    com.param[15] = 0;
    com.param[16] = 1;
    com.param[17] = 150; // z
  
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);


    com.lparam = 7700; // rgb color - 77% green
    // sizes, x,y,z, mm
    com.param[0]  = 240;
    com.param[1]  = 300;
    com.param[2]  = 200;
    // local sistem of coordinate, x,y,z, mm, usually in the center
    com.param[3]  = 0;
    com.param[4]  = 0;
    com.param[5]  = 0;
    // frame - coordinates of local frame in global system
    com.param[6]  = 1;
    com.param[7]  = 0;
    com.param[8]  = 0;
    com.param[9]  = 0; // x
    com.param[10] = 0;
    com.param[11] = 1;
    com.param[12] = 0;
    com.param[13] = 1000; // y
    com.param[14] = 0;
    com.param[15] = 0;
    com.param[16] = 1;
    com.param[17] = 350; // z
  
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);


    com.lparam = 222299; // rgb color - lightblue
    // sizes, x,y,z, mm
    com.param[0]  = 100;
    com.param[1]  = 100;
    com.param[2]  = 100;
    // local sistem of coordinate, x,y,z, mm, usually in the center
    com.param[3]  = 0;
    com.param[4]  = 0;
    com.param[5]  = 0;
    // frame - coordinates of local frame in global system
    com.param[6]  = 1;
    com.param[7]  = 0;
    com.param[8]  = 0;
    com.param[9]  = 0; // x
    com.param[10] = 0;
    com.param[11] = 1;
    com.param[12] = 0;
    com.param[13] = 1000; // y
    com.param[14] = 0;
    com.param[15] = 0;
    com.param[16] = 1;
    com.param[17] = 500; // z
  
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);


    // taking small box by small gripper
    take_small_gripper(robot);
    move_rpy_to( robot, 0, 1000, 950, 0, 1.57, 1.57); 
    move_rpy_to( robot, 0, 1000, 750, 0, 1.57, 1.57); 

    open_gripper(robot, 100);

    com.command = GRASP_OBJECT;
    com.lparam = 3;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);

    move_rpy_to( robot, 0, 1000, 800, 0, 1.57, 1.57); 
    move_rpy_to( robot, 1000, 1000, 900, 0, 1.57, 1.57); 
    move_rpy_to( robot, 1000, 1000, 300, 0, 1.57, 1.57); 
    open_gripper(robot, 200);

    com.command = RELEASE_OBJECT;
    com.lparam = 3;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);

    move_rpy_to( robot, 1000, 1000, 900, 0, 1.57, 1.57); 
    put_small_gripper(robot);

    // taking the biggest box with vacuum

    take_vacuum_gripper(robot);
    move_rpy_to( robot, 0, 1000, 900, 0, 1.57, 0.8); 
    move_rpy_to( robot, 0, 1000, 675, 0, 1.57, 0.8); 

    com.command = GRASP_OBJECT;
    com.lparam = 2;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);

    move_rpy_to( robot, 0, 1000, 775, 0, 1.57, 0.8); 
    move_rpy_to( robot, -300, 1000, 800, 0, 1.57, 0.8); 
    move_rpy_to( robot, -300, 1000, 475, 0, 1.57, 0.8); 
   
    com.command = RELEASE_OBJECT;
    com.lparam = 2;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);

    move_rpy_to( robot, 0, 1000, 900, 0, 1.57, 0.8); 
    put_vacuum_gripper(robot);

    // taking yellow box with big gripper

    take_big_gripper(robot);

    move_rpy_to( robot, 0, 1300, 900, 0, 1.57, 0); 
    move_rpy_to( robot, 0, 1300, 450, 0, 1.57, 0); 
    open_gripper( robot, 200);

    com.command = GRASP_OBJECT;
    com.lparam = 1;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);

    move_rpy_to( robot, 0, 1300, 800, 0, 1.57, 0); 
    move_rpy_to( robot, -300, 1100, 800, 0, 1.57, 0); 
    move_rpy_to( robot, -300, 1100, 650, 0, 1.57, 0); 
   
    open_gripper(robot, 400);

    com.command = RELEASE_OBJECT;
    com.lparam = 1;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);

    move_rpy_to( robot, -300, 1100, 900, 0, 1.57, 0); 
    put_big_gripper(robot);

    // putting small box on the top of the piramid, #:0)

    take_small_gripper(robot);

    move_rpy_to( robot, 1000, 1000, 900, 0, 1.57, 0); 
    move_rpy_to( robot, 1000, 1000, 300, 0, 1.57, 0); 
    open_gripper( robot, 100);
    
    com.command = GRASP_OBJECT;
    com.lparam = 3;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);

    move_rpy_to( robot, 1000, 1000, 800, 0, 1.57, 0); 
    move_rpy_to( robot, -300, 1000, 900, 0, 1.57, 0); 
    move_rpy_to( robot, -300, 1000, 750, 0, 1.57, 0); 
    open_gripper( robot, 250);

    com.command = RELEASE_OBJECT;
    com.lparam = 3;
    any_command <<= com;
    while( robot->put_command(any_command) == -1 ) sleep(1);
    
    move_rpy_to( robot, -300, 1000, 900, 0, 1.57, 0); 
    
    put_small_gripper(robot);
    go_home(robot);
    sleep(2);

    // some kind of joke, #:0)
    draw_end(robot);
    
    sleep(10);
  }

  return 0;
}

