/*
* This file is part of OpenKC
*
* Copyright(c) Matthias Schöpfer (mschoepf@techfak.uni-bielefeld.de)
* http://opensource.cit-ec.de/projects/openkc
*
* This file may be licensed under the terms of of the
* GNU Lesser General Public License Version 3 (the ``LGPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the LGPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the LGPL along with this
* program. If not, go to http://www.gnu.org/licenses/lgpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*
*/

#include "fri_okc_comm.h"
#include "fri_okc_helper.h"

/* Change Hostname here (IP or Hostname allowed */
#define OKC_HOST "192.168.10.123" 
/* #define OKC_PORT "6075" /\* 6075-6083 is unassigned. 6008 is used by X *\/ */

#define OKC_PORT "49938" /* FRI Port */





int
callback_doing_axis_control_right (void* priv_p, 
				   const fri_float_t* axis_pos_act,
				   fri_float_t* new_axis_pos)


{

  okc_handle_t* okc = (okc_handle_t*) priv_p;

  if ((OKC_OK != okc_is_robot_in_command_mode (okc,0)) ||
      (OKC_OK != okc_is_power_on (okc,0)))
    {
      okc_cp_lbr_mnj (axis_pos_act, new_axis_pos);

      return (OKC_OK);
    }
  
  if (OKC_OK != okc_get_jntpos_act (okc, 0, new_axis_pos))
    {
      
      okc_cp_lbr_mnj (axis_pos_act, new_axis_pos);
      
      
      return (OKC_OK);
    }

  

  return (OKC_OK);
}

int
callback_doing_axis_control_left (void* priv_p, 
				  const fri_float_t* axis_pos_act,
				  fri_float_t* new_axis_pos)


{

  okc_handle_t* okc = (okc_handle_t*) priv_p;

  if ((OKC_OK != okc_is_robot_in_command_mode (okc,0)) ||
      (OKC_OK != okc_is_power_on (okc,0)))
    {
      okc_cp_lbr_mnj (axis_pos_act, new_axis_pos);

      return (OKC_OK);
    }
  
  if (OKC_OK != okc_get_jntpos_act (okc, 0, new_axis_pos))
    {
      
      okc_cp_lbr_mnj (axis_pos_act, new_axis_pos);
      
      
      return (OKC_OK);
    }

  new_axis_pos[0] *= -1.0;

  return (OKC_OK);
}


int
callback_doing_cp_and_joint_control (void* priv_p, 
				     const fri_float_t* cart_pos_act,
				     const fri_float_t* jnt_pos_act,
				     fri_float_t* new_cart_pos,
				     fri_float_t* new_jnt_pos)

{

  okc_handle_t* okc = (okc_handle_t*) priv_p;

  if ((OKC_OK != okc_is_robot_in_command_mode (okc,0)) ||
      (OKC_OK != okc_is_power_on (okc,0)))
    {
      okc_cp_cart_frm_dim (cart_pos_act,new_cart_pos);
      okc_cp_lbr_mnj (jnt_pos_act, new_jnt_pos);

      return (OKC_OK);
    }
  
  if ((OKC_OK != okc_get_position_act (okc, 0, (transform_t*)new_cart_pos)) ||
      (OKC_OK != okc_get_jntpos_act (okc, 0, new_jnt_pos)))
    {
      okc_cp_cart_frm_dim (cart_pos_act,new_cart_pos);
      okc_cp_lbr_mnj (jnt_pos_act, new_jnt_pos);

      return (OKC_OK);
    }

  return (OKC_OK);
}

static void
_set_stiffness_damping (okc_handle_t* okc)
{
  
  lbr_axis_t astiff, adamp;
  coords_t kstiff, kdamp;

  astiff.a1 = 1000.0;
  astiff.a2 = 1000.0;
  astiff.a3 = 1000.0;
  astiff.a4 = 1000.0;
  astiff.a5 = 1000.0;
  astiff.a6 = 1000.0;
  astiff.e1 = 1000.0;
  
  adamp.a1 = 0.7;
  adamp.a2 = 0.7;
  adamp.a3 = 0.7;
  adamp.a4 = 0.7;
  adamp.a5 = 0.7;
  adamp.a6 = 0.7;
  adamp.e1 = 0.7;

  if (OKC_OK != okc_set_axis_stiffness_damping (okc,0,astiff,adamp))
    exit (EXIT_FAILURE);

  kstiff.x = 1000.0;
  kstiff.y = 1000.0;
  kstiff.z = 1000.0;
  kstiff.a = 300.0;
  kstiff.b = 300.0;
  kstiff.c = 300.0;
  
  kdamp.x = 0.7;
  kdamp.y = 0.7;
  kdamp.z = 0.7;
  kdamp.a = 0.7;
  kdamp.b = 0.7;
  kdamp.c = 0.7;

  if (OKC_OK != okc_set_cp_stiffness_damping (okc,0,kstiff,kdamp))
    exit (EXIT_FAILURE);

}

static int
keypressed (void)
{
  fd_set readfds;
  struct timeval timeout;
  
  FD_ZERO (&readfds);
  FD_SET (0,&readfds);
  timeout.tv_sec = 0;
  timeout.tv_usec = 0;
  
  return select (1,&readfds,NULL,NULL,&timeout);
}


int 
main (int argc, char** argv)
{
  okc_handle_t* okc;
  int cmdFlags;
  int quality = FRI_QUALITY_UNACCEPTABLE;

  /* okc = okc_start_server (OKC_HOST,OKC_PORT, */
  /* 			  OKC_MODE_CALLBACK_POS_AXIS_ABS);*/

  okc = okc_start_server (OKC_HOST,OKC_PORT,
			  OKC_MODE_CALLBACK_AXIS_ABS);

  if (NULL == okc)
    {
      printf ("unable to start server\n");
      exit (EXIT_FAILURE);
    }

  /* okc_register_cartpos_axis_set_absolute_callback  */
  /*   (okc,0, callback_doing_cp_and_joint_control, (void*) okc); */

  okc_register_axis_set_absolute_callback 
    (okc,0, callback_doing_axis_control_right, (void*) okc);
  okc_register_axis_set_absolute_callback 
    (okc,1, callback_doing_axis_control_left, (void*) okc);
  
						   

  printf ("waiting for robot to connect\n");

  while ((OKC_OK != okc_is_robot_avail (okc,0)) ||
	 (OKC_OK != okc_is_robot_avail (okc,1)))
    {
      sleep (1);
    }

  fprintf (stderr,"waiting for decent connection quality . . . ");

  while ((FRI_QUALITY_OK != quality) && (FRI_QUALITY_PERFECT != quality) )
    {
      sleep(1);
      okc_get_connection_quality (okc,1,&quality);
    }
  
  cmdFlags = FRI_CMD_JNTPOS /* | FRI_CMD_CARTPOS */;

  if (OKC_OK != okc_alter_cmdFlags (okc,0,cmdFlags))
    exit (EXIT_FAILURE);

  fprintf (stderr,"done\nSwitching to Axis Impedance . . . ");
	  
  if (OKC_OK != okc_switch_to_axis_impedance (okc,0))
    exit (EXIT_FAILURE);
  if (OKC_OK != okc_switch_to_axis_impedance (okc,1))
    exit (EXIT_FAILURE);
  
  fprintf (stderr,"done\nSetting Stiffness & Damping . . .");
  
  _set_stiffness_damping (okc);

  fprintf (stderr,"done\nRequesting command mode . . . ");

  if (OKC_OK != okc_request_command_mode (okc,0))
    {
      fprintf (stderr,"\nFailed to request command mode\n");
      exit (EXIT_FAILURE);
    }

  fprintf (stderr,"done\nRequesting command mode . . . ");

  if (OKC_OK != okc_request_command_mode (okc,1))
    {
      fprintf (stderr,"\nFailed to request command mode\n");
      exit (EXIT_FAILURE);
    }

  while (OKC_OK != okc_is_robot_in_command_mode (okc,1))
    {
      if (OKC_OK != okc_request_command_mode (okc,1))
	{
	  fprintf (stderr,"Some error occured. Bailing out\n");
	  exit (EXIT_FAILURE);
	}
      sleep(1);
    }

  while (OKC_OK != okc_is_robot_in_command_mode (okc,0))
    {
      if (OKC_OK != okc_request_command_mode (okc,0))
	{
	  fprintf (stderr,"Some error occured. Bailing out\n");
	  exit (EXIT_FAILURE);
	}
      sleep(1);
    }

  

  fprintf (stderr,"Waiting for drives to power on . . . ");
  while (OKC_OK != okc_is_power_on (okc,0))
    sleep (1);

   while (OKC_OK != okc_is_power_on (okc,1))
    sleep (1);

  fprintf (stderr,"done\n");

  while (0 == keypressed())
    {
      sleep (1);

      printf ("Statistics:\n");
      
      okc_print_robot_informations (okc,0);
    }

  printf ("stopping server\n");

  okc_stop_server (okc);

  return 0;
}