/* tripdemo.c

	Serial Trippy demo and exercise program
	requires firmware version 0.16 or above

        Modified Jan 2000, by Douglas W. Jones

	Linux version (may also run under NT Posix environment with no mods)

	From the shell:

	   tripdemo [ <options> ]

	<options> may include:

	   -nowait suppresses pause after each move
	   -square forces moves to be rectilinear
	   -pan xx, where xx defaults to 130, pan range
	   -tilt xx, where xx defaults to 30, tilt range
	   -zoom xx, where xx defaults to 10, percent of moves that zoom
	   <serial port> defaults to SERIALPORT

	The Trippy will move randomly within the defined
	angular range, straying no more than -pan degrees from
	straight ahead and no more than -tilt degrees from
	horizontal.
*/

#define SERIALPORT "/dev/ttyS0"

/* Trippy firmware version to be tested */

#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <termios.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>

/************************************
 * package to support I/O to Trippy * 
 ************************************/

int tripin, tripout;
	/* these are set by tripopen */

void tripopen( f )
char * f;
{
	tripin = open( f, O_RDONLY /* | O_NDELAY */ );
	if (tripin < 0) {
		perror( "open trippy input file failed," );
		exit(0);
	}

	/* configure trippy input file */
	{
		struct termios term;
		int stat;
		term.c_iflag = PARMRK | INPCK;
		term.c_oflag = 0;
		term.c_cflag = CS8 | CREAD | PARENB | PARODD | CLOCAL;
		term.c_lflag = 0;
		(void) cfsetospeed( &term, B9600 );
		(void) cfsetispeed( &term, B9600 );
		stat = tcsetattr( tripin, TCSANOW, &term );
		if (stat < 0) {
			perror( "tcsetattr failed," );
			exit(0);
		}
	}

	/* input is now 8-bit odd parity 9600 baud raw I/O */
	/* parity error reads as [RUB] NULL, break reads as NULL */

	tripout = open( f, O_WRONLY );
	/* why not use tripout = dup(tripin); */

	/* configure trippy output file */
	if (tripout < 0) {
		perror( "open tripout failed," );
		exit(0);
	}
}

void tripputs( s )
char * s;
/* put string s out to Trippy */
{
	int outlen = strlen( s );
	int stat;
	stat = write( tripout, s, outlen );
	if (stat < 0) {
		perror( "write failure in tripputs()" );
		exit( 0 );
	}
}

char tripgetc()
/* await and return one character from Trippy */
{
	int stat;
	char ch;
	stat = read( tripin, &ch, 1 );
	if (stat < 0) {
		perror( "\nRead failed," );
		exit( 0 );
	}
	return( ch );
}

char tripgets( s, size )
char * s;
int size;
/* await and return \n terminated line s from Trippy, strlen(s) < size */
{
	for (;;) {
		char ch;
		if (size <= 1) break;
		ch = tripgetc(ch);
		*s = ch;
		s++;
		size--;
		if (ch == '\n') break;
	}
	if (size > 0) *s = '\0';
}


/*************************************************************
 * package to print strings with printable control chars etc * 
 *************************************************************/

int printable; /* was most recent character printable? */

void putoutbegin( s )
char *s;
{
	if (s) fputs( s, stderr );
	printable = 0;
}

void putoutend( s )
char *s;
{
	if (printable) fputs( "\" ", stderr );
	if (s) fputs( s, stderr );
}

void putoutchar( ch )
char ch;
{
	int highbit = 0;
	if (ch & 0x80) { /* first strip off 8th bit into highbit */
		if (printable) fputs( "\" ", stderr );
		highbit = 1;
		printable = 0;
		ch = ch & 0x7F;
	}
	if (ch < 0x20) { /* we have control char, output by name */
		static char *cc_name[] = {
			"NULL", "SOH", "STX", "ETX",
			"EOT", "ENQ", "ACK", "BEL",
			"BS", "HT", "LF", "VT",
			"FF", "CR", "SO", "SI",
			"DLE", "DC1", "DC2", "DC3",
			"DC4", "NAK", "SYN", "ETB",
			"CAN", "EM", "SUB", "ESC",
			"FS", "GS", "RS", "US"
		};
		if (printable) fputc( '"', stderr );
		fputc( ' ', stderr );
		if (highbit) fputc( '[', stderr );
		fputs( cc_name[ch], stderr ); 
		if (highbit) fputc( ']', stderr );
		printable = 0;
	} else if (ch <= '~') {	/* we have printable text */
		if (highbit) {
			if (printable) fputc( '"', stderr );
			fputs( " [", stderr );
			fputc( ch, stderr );
			fputc( ']', stderr );
			printable = 0;
		} else {
			if (!printable) fputs( " \"", stderr );
			if ((ch == '"')||(ch == '\\'))
				fputc( '\\', stderr );
			fputc( ch, stderr );
			printable = 1;
		}
	} else { /* we have rubout */
		if (printable) fputc( '"', stderr );
		fputc( ' ', stderr );
		if (highbit) fputc( '[', stderr );
		fputs( "RUB", stderr );
		if (highbit) fputc( ']', stderr );
		printable = 0;
	}
}

void putouts( s )
char * s;
{
	while (*s) {
		putoutchar( *s );
		s++;
	}
}



/*****************************************
 * Trippy high level I/O support routine *
 *****************************************/

void tripgeteol( s )
char * s; /* error message to send if EOL not found */
{
	char ch;
	ch = tripgetc();
	if (ch != '\n') {
		fputs( s, stderr );
		putoutbegin( "Received:" );
		do {
			putoutchar( ch );
			ch = tripgetc();
		} while (ch != '\n');
		putoutend( "\n" );
	}
}

void tripputvar( v, i )
char v; /* the variable to assign to */
int  i; /* the value to assign */
{
	char com[40];
	char ch;
	int ii = i;
	int pos = 0;

	if (i < 0); ii = -ii;
	pos += sprintf( &com[pos], "BT%1c%1d", v, ii );
	if (i < 0) pos += sprintf( &com[pos], "-" );
	pos += sprintf( &com[pos], "=\n\0" );
	tripputs( com );
	tripgeteol( "Unexpected trippy reply in tripputvar\n" );
}

tripgetvar( v )
char v; /* the variable to assign to */
{
	char com[40];
	char resp[40];
	char ch;
	int i;
	int stat;
	int pos = 0;

	pos += sprintf( &com[pos], "BT%1c?\n\0", v );
	tripputs( com );
	tripgets( resp, 40 );
	stat = sscanf( resp, "=%d\n", &i );
	if (stat < 1) {
		fputs( "Unexpected trippy reply in tripgetvar", stderr );
		putoutbegin( "Sent:" );
		putouts( com );
		putoutend( "\n" );
		putoutbegin( "Received:" );
		putouts( resp );
		putoutend( "\n" );
	}
	return i;
}


/*************************
 * Trippy motion package *
 *************************/

int trip_var_m; /* maximum x coordinate in steps */
int trip_var_n; /* minimum x coordinate in steps */
int trip_var_o; /* maximum y coordinate in steps */
int trip_var_p; /* minimum y coordinate in steps */
int trip_var_s; /* x steps per circle */
int trip_var_t; /* y steps per circle */

void trip_move_init()
/* find out motion parameters */
{
	trip_var_m = tripgetvar( 'M' );
	trip_var_n = tripgetvar( 'N' );
	trip_var_o = tripgetvar( 'O' );
	trip_var_p = tripgetvar( 'P' );
	trip_var_s = tripgetvar( 'S' );
	trip_var_t = tripgetvar( 'T' );
}

void trip_move_to( x, y )
int x; /* x coordinate in degrees */
int y; /* y coordinate in degrees */
{
	/* convert coordinates from degrees to steps */
	int xx = (x * trip_var_s) / 360;
	int yy = (y * trip_var_t) / 360;

	/* check bounds (the Trippy does this too, but it doesn't complain) */
	if ((xx > trip_var_m) || (xx < trip_var_n))
		fputs( "trip_move_to x out of bounds\n", stderr );
	if ((yy > trip_var_o) || (yy < trip_var_p))
		fputs( "trip_move_to y out of bounds\n", stderr );

	/* move */
	tripputvar( 'X', xx );
	tripputvar( 'Y', yy );
}

void trip_wait_move_done()
{
	tripputs( "BT!\n" );
	tripgeteol( "Unexpected trippy reply in trip_wait_move_done\n" );
}

/***********************
 * Trippy zoom package *
 ***********************/

int trip_var_q; /* maximum z coordinate */
int trip_var_r; /* minimum z coordinate */

void trip_zoom_init()
/* find out zoom parameters */
{
	trip_var_q = tripgetvar( 'Q' );
	trip_var_r = tripgetvar( 'R' );
}

void trip_zoom_to( z )
int z; /* z coordinate in percent */
{
	/* convert coordinates from percent to zoom units */
	int zz = trip_var_r + ((z * (trip_var_q - trip_var_r)) / 100);

	/* move */
	tripputvar( 'Z', zz );
}

void trip_wait_zoom_done()
{
	tripputs( "BT!\n" );
	tripgeteol( "Unexpected trippy reply in trip_wait_zoom_done\n" );
}

main (argc, argv)
int argc;
char * argv[];
{
	char * filename = NULL;
	int argno = 1;
	int nowait = 0;
	int square = 0;
	int pan = 130;
	int tilt = 30;
	int zoom = 10;
	while (argno < argc) {
		if (argv[argno][0] == '-') { /* options */
			if (strcmp(argv[argno], "-nowait") == 0) {
				nowait = 1;
			} else if (strcmp(argv[argno], "-square") == 0) {
				square = 1;
			} else if (strcmp(argv[argno], "-pan") == 0) {
				argno++;
				if (argno < argc) {
					pan = atoi( argv[argno] );
				}
			} else if (strcmp(argv[argno], "-tilt") == 0) {
				argno++;
				if (argno < argc) {
					tilt = atoi( argv[argno] );
				}
			} else if (strcmp(argv[argno], "-zoom") == 0) {
				argno++;
				if (argno < argc) {
					zoom = atoi( argv[argno] );
				}
			} else { /* option not recognized */
				puts(
				"tripdemo [ <options> ]\n"
				"  -nowait suppresses pause after each move\n"
				"  -square forces alternating tilts and pans\n"
				"  -pan 130 pans over +/-130 degrees\n"
				"  -tilt 30 tilts through +/-30 degrees\n"
				"  -zoom 30 changes zoom on 30% of moves\n"
				"  <serial port> defaults to "SERIALPORT"\n"
				"Demonstrates Trippy attached to given port\n"
				);
				exit(0);
			}
		} else {
			if (filename == NULL) {
				filename = argv[argno];
			} else {
				puts(
				"only one file name allowed!\n"
				"try tripinit -help\n"
				);
				exit(0);
			}
		}
		argno++;
	}

	if (filename == NULL) {
		filename = SERIALPORT;
	}

	fputs( "\nOPENING: ", stdout );
	fputs( filename, stdout );
	fputs( "\n\n", stdout );

	tripopen( filename );

	fputs( "\nINITIALIZATION:\n\n", stdout );

	trip_move_init();
	trip_zoom_init();

	fputs( "\nEXERCISE:\n\n", stdout );

	for (;;) {
		int x, y, z;
		x = (random() % pan) - (random() % pan);
		if (square == 1) {
			trip_move_to( x, y );
			trip_wait_move_done();
			if (nowait == 0) sleep(1);
		}
		y = (random() % tilt) - (random() % tilt);
		trip_move_to( x, y );
		if ((nowait == 0)||(square == 1)) trip_wait_move_done();
		if (nowait == 0) sleep(1);

		if ((random() % 100) < zoom) {
			z = (random() % 100);
			trip_zoom_to( z );
			trip_wait_zoom_done( z );
		}
	}
}