/*
* 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"


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

  okc_cp_cart_frm_dim (cart_pos_act,new_cart_pos);
  return (OKC_OK);    
}

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 quality = FRI_QUALITY_UNACCEPTABLE;
  unsigned int n = 0;
  double mean = 0.0;
  double M2 = 0.0;
  double delta;
  coords_t k;

  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_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);
    }
  
  fprintf (stderr,"done\n");


  while (0 == keypressed())
    {
      okc_sleep_cycletime (okc,0);
      okc_get_ft_tcp_est (okc,0,&k);
      n++;
      delta = k.z - mean;
      mean += delta/(double)n;
      M2 += delta*(k.z - mean);
      fprintf (stderr,"\rMean: %f, Stddev: %f                               ",
	       mean,M2/(double)n);
      
    }

  printf ("stopping server\n");

  okc_stop_server (okc);

  return 0;
}