
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <sys/time.h>

#include <ypspur.h>


int gShutoff = 0;
void ctrlc( int aN )
{
	gShutoff = 1;
	signal( SIGINT, NULL );
}


double get_time( void )
{
  struct timeval current;

  gettimeofday(&current, NULL);
  
  return  current.tv_sec + current.tv_usec/1000000.0;   
}

int main( int argc, char *argv[] )
{
	FILE *fp;
	pid_t pid;
	char filename[512];
	
	if( argc < 2 )
	{
		fprintf( stderr, "USAGE: %s program\n", argv[ 0 ] );
		return 1;
	}
	
	if( ( pid = fork() ) == 0 )
	{
		execv( argv[1], &argv[2] );
	}

	sprintf( filename, "%s.dat", argv[1] );	
	fp = fopen( filename, "w" );
	
	Spur_init();
	fprintf( fp, "# Xbs Ybs THbs Xgl Ygl THgl Xlc Ylc THlc time\n" );
	while( !gShutoff )
	{
		double x, y, th;
		double t;
		int status;

		Spur_get_pos_BS( &x, &y, &th );
		fprintf( fp, "%f %f %f  ", x, y, th );
		Spur_get_pos_GL( &x, &y, &th );
		fprintf( fp, "%f %f %f  ", x, y, th );
		Spur_get_pos_LC( &x, &y, &th );
		fprintf( fp, "%f %f %f  ", x, y, th );
		
		t = get_time();
		fprintf( fp, "%f\n", t );

		usleep( 50000 );
		if( waitpid( pid, &status, WNOHANG ) > 0 ) break;
	}
	
	fclose( fp );

	return 1;
}


