/*
* 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 <stdlib.h>
#include <stdio.h>
#include <signal.h>
#include <time.h>
/* #include <unistd.h> */
#include <sys/time.h>
#include <errno.h>
#include <okc_types.h>
#include <okc_comm.h>
#include <gsl/gsl_statistics.h>



static void 
move_all_joints_all_dirs(okc_handle_t* okc) 
{
  coords k;
  lbr_axis la;
  struct timespec ts;
  double *p;
  int i;
  p = (double*) &la;
  for (i = 0; i < 7; i++)
    {
      p[i] = 0.1;
      okc_set_axis_correction (okc,0,la);
      ts.tv_sec = 0;
      ts.tv_nsec = 200000000;
      nanosleep (&ts,NULL);
      p[i] = 0.00;
      okc_set_axis_correction (okc,0,la);
    }
  for (i = 0; i < 7; i++)
    {
      p[6 - i] = -0.10;
      okc_set_axis_correction (okc,0,la);
      ts.tv_sec = 0;
      ts.tv_nsec = 200000000;
      nanosleep (&ts,NULL);
      p[6 - i] = 0.00;
      okc_set_axis_correction (okc,0,la);
    }
  
  p = (double*) &k;
  for (i = 0; i < 6; i++)
    {
      p[i] = 0.2;
      okc_set_coords_correction (okc,0,k);
      ts.tv_sec = 0;
      ts.tv_nsec = 200000000;
      nanosleep (&ts,NULL);
      p[i] = 0.00;
      okc_set_coords_correction (okc,0,k);
    }
  for (i = 0; i < 6; i++)
    {
      p[5 - i] = -0.2;
      okc_set_coords_correction (okc,0,k);
      ts.tv_sec = 0;
      ts.tv_nsec = 200000000;
      nanosleep (&ts,NULL);
      p[5 - i] = 0.00;
      okc_set_coords_correction (okc,0,k);
    }
}

int
main (int argc, char** argv)
{
  okc_handle_t* okc;
  coords k;
  
 
  okc = okc_start_server ("192.168.10.123","6075",OKC_MODE_SETTER);

  if (NULL == okc)
    {
      fprintf (stderr,"Could not create OKC-Server\n");
      exit (EXIT_FAILURE);
    }

  okc_coords_set_all_zero (&k);

  /* while (OKC_OK != okc_is_robot_avail (okc,0)) */
/*     sleep (1); */

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

  sleep (2);

  fprintf (stderr,"Setting Cartesian Impedance . . . ");

  k.x = 200.0;
  k.y = 2000.0;
  k.z = 1000.0;
  k.a = 100.0;
  k.b = 100.0;
  k.c = 100.0;
  
  okc_set_cp_stiffness (okc,0,k);
  
  k.x = 0.7;
  k.y = 0.7;
  k.z = 0.7;
  k.a = 0.7;
  k.b = 0.7;
  k.c = 0.7;
  okc_set_cp_damping (okc,0,k);
  okc_switch_to_cp_impedance (okc,0);

  move_all_joints_all_dirs (okc);

  fprintf (stderr,"Applying force . . . ");
  okc_set_desiredforce_params (okc,0,0,0,25.0,25.0,0.5,2.0,0.5,0.0,0.0);
  okc_enable_desiredforce (okc,0,OKC_X_AXIS);
  fprintf (stderr,"done\n");
  move_all_joints_all_dirs (okc);
  sleep (6);
  fprintf (stderr,"Removing force . . . ");
  okc_disable_desiredforce(okc,0);
  fprintf (stderr,"done\n");
  sleep (1);


  fprintf (stderr,"Setting position Control . . . ");
  okc_switch_to_position (okc,0);
  fprintf (stderr,"done\n");

  fprintf (stderr,"Exiting . . . ");

  okc_stop_server (okc);

  fprintf (stderr,"done\n");
  
  return 0;
}