/*
* 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 <fri_okc_types.h>
#include <fri_okc_comm.h>
#include <gsl/gsl_statistics.h>


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;
  coords k;
  lbr_axis lbr;

 
  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) || 
	 OKC_OK != okc_is_robot_avail (okc,1))
    sleep (1);

  sleep (2);

  while (0 == keypressed())
    {
      lbr.a1 = -90.0;
      lbr.a2 = 107.3;
      lbr.a3 = -84.92;
      lbr.a4 = 0.0;
      lbr.a5 = 77.8;
      lbr.a6 = 0.0;
      lbr.e1 = 0.0;
      fprintf (stderr,"drive!\n");
      
#pragma omp parallel sections
      {
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,0,lbr);
	}
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,1,lbr);
	}
      }

      lbr.a1 = -90.0;
      lbr.a2 = 116.70;
      lbr.a3 = -98.0;
      lbr.a4 = 0.0;
      lbr.a5 = 53.40;
      lbr.a6 = 0.0;
      lbr.e1 = 0.0;
      fprintf (stderr,"drive!\n");

#pragma omp parallel sections
      {
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,0,lbr);
	}
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,1,lbr);
	}
      }

      lbr.a1 = -90.0;
      lbr.a2 = 107.3;
      lbr.a3 = -84.92;
      lbr.a4 = 0.0;
      lbr.a5 = 77.8;
      lbr.a6 = 0.0;
      lbr.e1 = 0.0;
      
#pragma omp parallel sections
      {
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,0,lbr);
	}
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,1,lbr);
	}
      }


      lbr.a1 = 90.0;
      lbr.a2 = 107.3;
      lbr.a3 = -84.92;
      lbr.a4 = 0.0;
      lbr.a5 = 77.8;
      lbr.a6 = 0.0;
      lbr.e1 = 0.0;
      

#pragma omp parallel sections
      {
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,0,lbr);
	}
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,1,lbr);
	}
      }

 
      lbr.a1 = 90.0;
      lbr.a2 = 116.70;
      lbr.a3 = -98.0;
      lbr.a4 = 0.0;
      lbr.a5 = 53.40;
      lbr.a6 = 0.0;
      lbr.e1 = 0.0;

#pragma omp parallel sections
      {
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,0,lbr);
	}
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,1,lbr);
	}
      }


      lbr.a1 = 90.0;
      lbr.a2 = 107.3;
      lbr.a3 = -84.92;
      lbr.a4 = 0.0;
      lbr.a5 = 77.8;
      lbr.a6 = 0.0;
      lbr.e1 = 0.0;
      
#pragma omp parallel sections
      {
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,0,lbr);
	}
#pragma omp section
	{
	  okc_send_drive_ptp_axis (okc,1,lbr);
	}
      }      
    }

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

  okc_stop_server (okc);

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