/*
* 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"
#include "fri_okc_hostconfig.h"


static int
in_bounds (double x, double low, double high)
{
  if ((x > low) && (x < high))
    return 1;
  return 0;
}

static void
to_bounds (fri_float_t* x, int index, fri_float_t low, fri_float_t high)
{
  if (x[index] < low)
    x[index] = low;

  if (x[index] > high)
    x[index] = high;
}



int
callback_doing_cp_control (void* priv_p, 
			   const fri_float_t* cart_pos_act,
			   fri_float_t* new_cart_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);

      return (OKC_OK);
    }
  
  if (OKC_OK != okc_get_position_act (okc, 0, (transform_t*)new_cart_pos))
    {
      okc_cp_cart_frm_dim (cart_pos_act,new_cart_pos);
      
      
      
      return (OKC_OK);
    }

  /* fprintf (stderr,"bf\n"); */
  /* okc_print_cart_frm_dim (new_cart_pos); */

  to_bounds (new_cart_pos,3,-0.2,0.1);
  to_bounds (new_cart_pos,7,0.4, 0.6);
  to_bounds (new_cart_pos,11, 0.5, 0.7);
  
  /* okc_print_cart_frm_dim (new_cart_pos); */

  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 = 2000.0;
  kstiff.y = 2000.0;
  kstiff.z = 2000.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_POS_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_cartpos_set_absolute_callback
    (okc,0, callback_doing_cp_control, (void*) okc);
						   

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

  while (OKC_OK != okc_is_robot_avail (okc,0))
    {
      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,0,&quality);
    }
  
  cmdFlags = OKC_CMD_FLAGS_CP_IMPEDANCE_MODE;

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

  fprintf (stderr,"done\nSwitching to CP Impedance . . . ");
	  
  if (OKC_OK != okc_switch_to_cp_impedance (okc,0))
    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);
    }

  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);

  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;
}