#include <unistd.h>
#include <Command_Type.h>
#include "Sample_Calls.h"


/////////////////////////////////////////////////////////////
///////////////////  sample routines .. //////////////////////
///////////////////////////////////////////////////////////////

//
void move_joints_to( Robot_var& robot, 
		     double p0,
		     double p1,
		     double p2,
		     double p3,
		     double p4,
		     double p5,
		     double p6  ){

  CORBA::Any any_command;
  Robot_Command com;

  com.command = MOVE_JOINTS_TO; 
  com.param[0] = p0;
  com.param[1] = p1;
  com.param[2] = p2;
  com.param[3] = p3;
  com.param[4] = p4;
  com.param[5] = p5;
  com.param[6] = p6;
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);
}


//
void move_joints_by( Robot_var& robot, 
		     double p0,
		     double p1,
		     double p2,
		     double p3,
		     double p4,
		     double p5,
		     double p6  ){

  CORBA::Any any_command;
  Robot_Command com;

  com.command = MOVE_JOINTS_BY; 
  com.param[0] = p0;
  com.param[1] = p1;
  com.param[2] = p2;
  com.param[3] = p3;
  com.param[4] = p4;
  com.param[5] = p5;
  com.param[6] = p6;
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);
}


//
void move_joint_to( Robot_var& robot, int num, double p){

  CORBA::Any any_command;
  Robot_Command com;

  com.command = MOVE_JOINT_TO; 
  com.lparam = num; 
  com.param[num - 1] = p;
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);
 
}


//
void move_joint_by( Robot_var& robot, int num, double p){

  CORBA::Any any_command;
  Robot_Command com;

  com.command = MOVE_JOINT_BY; 
  com.lparam = num; 
  com.param[num - 1] = p;
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);
 
}


//
void move_rpy_to( Robot_var& robot, 
		  double p0,
		  double p1,
		  double p2,
		  double p3,
		  double p4,
		  double p5 ) {
  
  CORBA::Any any_command;
  Robot_Command com;
  
  com.command = MOVE_RPY_TO;
  com.param[0] = p0;
  com.param[1] = p1;
  com.param[2] = p2;
  com.param[3] = p3;
  com.param[4] = p4;
  com.param[5] = p5;
  any_command <<= com;
  while( robot->put_command(any_command) == -1) sleep(1);
}


//
void move_rpy_by( Robot_var& robot, 
		  double p0,
		  double p1,
		  double p2,
		  double p3,
		  double p4,
		  double p5 ) {
  
  CORBA::Any any_command;
  Robot_Command com;
  
  com.command = MOVE_RPY_BY;
  com.param[0] = p0;
  com.param[1] = p1;
  com.param[2] = p2;
  com.param[3] = p3;
  com.param[4] = p4;
  com.param[5] = p5;
  any_command <<= com;
  while( robot->put_command(any_command) == -1) sleep(1);
}


//
void take_big_gripper( Robot_var& robot) {
 
  CORBA::Any any_command;
  Robot_Command com;

  move_rpy_to(robot, 1000, 350, 700, 0, 1.57, 0);
  move_rpy_to(robot, 1000, 350, 650, 0, 1.57, 0);
  
  com.command = ATTACH_BIG_GRIPPER;
  any_command <<= com;
  while( robot->put_command(any_command) == -1) sleep(1);

  move_rpy_by(robot, 0, 0, 300, 0, 0, 0);
}


//
void put_big_gripper( Robot_var& robot) {
  
  CORBA::Any any_command;
  Robot_Command com;

  move_rpy_to( robot, 1000, 350, 950, 0, 1.57, 0);
  move_rpy_by( robot, 0, 0, -300, 0, 0, 0);

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

  move_rpy_by( robot, 0, 0, 50, 0, 0, 0);
}


//
void take_small_gripper( Robot_var& robot) {

 
  CORBA::Any any_command;
  Robot_Command com;
  
  move_rpy_to( robot, 1000, 150, 700, 0, 1.57, 0);
  move_rpy_by( robot, 0, 0, -50, 0, 0, 0);

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

  move_rpy_by( robot, 0, 0, 300, 0, 0, 0);
}


//
void put_small_gripper( Robot_var& robot) {
  
  CORBA::Any any_command;
  Robot_Command com;

  move_rpy_to( robot, 1000, 150, 950, 0, 1.57, 0);  
  move_rpy_by( robot, 0, 0,-300, 0, 0, 0);

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

  move_rpy_by( robot, 0, 0, 50, 0, 0, 0);
}


//
void take_vacuum_gripper( Robot_var& robot) {
  
  CORBA::Any any_command;
  Robot_Command com;

  move_rpy_to( robot, 1000, -100, 700, 0, 1.57, 0);  
  move_rpy_by( robot, 0, 0, -50, 0, 0, 0);

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

  move_rpy_by( robot, 0, 0, 300, 0, 0, 0);
}


//
void put_vacuum_gripper( Robot_var& robot) {
  
  CORBA::Any any_command;
  Robot_Command com;
    
  move_rpy_to( robot, 1000, -100, 950, 0, 1.57, 0);
  move_rpy_by( robot, 0, 0, -300, 0, 0, 0);

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

  move_rpy_by( robot, 0, 0, 50, 0, 0, 0);
}


//
void go_home( Robot_var& robot){

  move_joints_to( robot, 0, 0, 0, 0, 0, 0, 0);
}


//
void open_gripper( Robot_var& robot, double opening){

  // joint 7 corresponds to gripper opening
  move_joint_to( robot, 7, opening);
}



//
void draw_end( Robot_var& robot){

  CORBA::Any any_command;
  Robot_Command com;

  // draw "END"

  // "E"
  com.command = ADD_OBJECT;
  com.lparam = 770000; // rgb color - 99% red
  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 100;
  com.param[2]  = 1000;
  // 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]  = 1000; // x
  com.param[10] = 0;
  com.param[11] = 1;
  com.param[12] = 0;
  com.param[13] = 600; // y
  com.param[14] = 0;
  com.param[15] = 0;
  com.param[16] = 1;
  com.param[17] = 800; // z
  
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 300;
  com.param[2]  = 100;
  // pose
  com.param[13] = 800; // y
  com.param[17] = 1250; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 300;
  com.param[2]  = 100;
  // pose
  com.param[13] = 800; // y
  com.param[17] = 350; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);


  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 300;
  com.param[2]  = 100;
  // pose
  com.param[13] = 800; // y
  com.param[17] = 800; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  // "N"

  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 100;
  com.param[2]  = 1000;
  // pose
  com.param[13] = 1100; // y
  com.param[17] = 800; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);


  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 100;
  com.param[2]  = 600;
  // pose
  com.param[13] = 1300; // y
  com.param[17] = 800; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 100;
  com.param[2]  = 1000;
  // pose
  com.param[13] = 1500; // y
  com.param[17] = 800; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  // "D"

  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 100;
  com.param[2]  = 1000;
  // pose
  com.param[13] = 1800; // y
  com.param[17] = 800; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);
  
  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 200;
  com.param[2]  = 800;
  // pose
  com.param[13] = 2200; // y
  com.param[17] = 800; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 500;
  com.param[2]  = 100;
  // pose
  com.param[13] = 1900; // y
  com.param[17] = 1250; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  // sizes, x,y,z, mm
  com.param[0]  = 100;
  com.param[1]  = 500;
  com.param[2]  = 100;
  // pose
  com.param[13] = 1900; // y
  com.param[17] = 350; // z
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);

  take_small_gripper(robot);
  go_home(robot);
  
  move_joints_to( robot, 1.57, -0.3, -1.3, 0, 0, 0, 200); 
  move_rpy_to( robot, 600, 1300, 800, 1.57, 0, 0); 
  move_rpy_by( robot, 200, 0, 0, 0, 0, 0); 
  open_gripper( robot, 100);


  com.command = GRASP_OBJECT;
  com.lparam = 9;
  any_command <<= com;
  while( robot->put_command(any_command) == -1 ) sleep(1);
    
  move_rpy_by( robot, 0, 0, 0, 0.5, 0, 0);
  open_gripper(robot, 200);

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

  move_rpy_by( robot, -200, 0, 0, 0, 0, 0); 
  move_joints_to( robot, 1.57, -0.3, -1.3, 0, 0, 0, 250);
  move_joints_to( robot, 0, 0, 0, 0, 0, 0, 250);

  put_small_gripper(robot);
  go_home(robot);
}
