#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <sys/shm.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <signal.h>
#include <fcntl.h>
#include <termios.h>
#ifdef __sgi
#include <sys/schedctl.h>
#endif
#include "dyna-trackd.h"


struct _dyna_options
	{
	char *portname;
	int shmemKey;
	float offset[3];
	float xRot;
	float xRotCos, xRotSin;
	enum { DYNA_RETRO, DYNA_STEREO, DYNA_STEREO60 } mode;
	int filterSmooth;
	float filterLock;
	int otherTrackerKey;
	};

void parse_options(int argc,char **argv,struct _dyna_options *options);
void dyna_init_tracker(struct _dyna_options *options);
int dyna_read_data(CAVE_SENSOR_ST *sensor,struct _dyna_options *options);
int dyna_filter_reading(CAVE_SENSOR_ST *sensor,struct _dyna_options *options);
void dyna_handle_command(int cmd,struct _dyna_options *options);
void dyna_reset_tracker(struct _dyna_options *options);
void trackd_cleanup(int);
void get_tracker_mem(int shmemKey);
void init_tracker_data(void);
void get_timestamp(uint32_t stamp[2]);
void attach_other_tracker_mem(int otherTrackerKey);
void check_other_tracker(struct TRACKD_TRACKING	*tracker);

static int dyna_fd;
static struct TRACKD_TRACKING	*tracker=NULL;

main(int argc,char **argv)
	{
	struct _dyna_options options;
	signal(SIGINT,trackd_cleanup); /* catch interrupts to close nicely */
#ifdef __sgi
	schedctl(NDPRI,0,NDPHIMIN);  /* set a non-degrading priority */
#endif
	parse_options(argc,argv,&options);
	get_tracker_mem(options.shmemKey);
	init_tracker_data();
	dyna_init_tracker(&options);
	if (options.otherTrackerKey)
		attach_other_tracker_mem(options.otherTrackerKey);

	while (1)
		{
		fd_set fdset;
		struct timeval timeout;
		FD_ZERO(&fdset);
		FD_SET(dyna_fd, &fdset);
		timeout.tv_sec = 0;
		timeout.tv_usec = 1000;
		select(32, &fdset, NULL, NULL, &timeout);
		if (FD_ISSET(dyna_fd, &fdset))
			{
			CAVE_SENSOR_ST sensor;
			if (dyna_read_data(&sensor,&options))
				{
				tracker->sensor[0] = sensor;
				get_timestamp(tracker->header.timestamp);
				}
			}
		if (options.otherTrackerKey)
			check_other_tracker(tracker);
		if (tracker->header.command)
			{
			dyna_handle_command(tracker->header.command,&options);
			tracker->header.command = 0;
			}
		}
	}


void parse_options(int argc,char **argv,struct _dyna_options *options)
	{
	int i;
	options->shmemKey = 9900;
#ifdef __sgi
	options->portname = strdup("/dev/ttyd2");
#else
	options->portname = strdup("/dev/ttyS0");
#endif
	options->offset[0] = options->offset[1] = options->offset[2] = 0;
	options->xRot = 0;
	options->otherTrackerKey = 0;
	options->mode = DYNA_RETRO;
	options->filterSmooth = 0;
	options->filterLock = 0;
	for (i=1; i < argc; i++)
		{
		if ((!strcmp(argv[i],"--shmem")) && (i < argc-1))
			{
			options->shmemKey = atoi(argv[++i]);
			}
		else if ((!strcmp(argv[i],"--port")) && (i < argc-1))
			{
			options->portname = strdup(argv[++i]);
			}
		else if ((!strcmp(argv[i],"--othertracker")) && (i < argc-1))
			{
			options->otherTrackerKey = atoi(argv[++i]);
			}
		else if ((!strcmp(argv[i],"--offset")) && (i < argc-3))
			{
			options->offset[0] = atof(argv[++i]);
			options->offset[1] = atof(argv[++i]);
			options->offset[2] = atof(argv[++i]);
			}
		else if ((!strcmp(argv[i],"--xrot")) && (i < argc-1))
			{
			options->xRot = atof(argv[++i]);
			}
		else if ((!strcmp(argv[i],"--mode")) && (i < argc-1))
			{
			i++;
			if (!strcmp(argv[i],"retro"))
				options->mode = DYNA_RETRO;
			else if ((!strcmp(argv[i],"stereo")) ||
				 (!strcmp(argv[i],"stereosync")))
				options->mode = DYNA_STEREO;
			else if ((!strcmp(argv[i],"stereo60")) ||
				 (!strcmp(argv[i],"stereosync60")))
				options->mode = DYNA_STEREO60;
			else
				fprintf(stderr,"WARNING: unknown mode \"%s\"\n",
					argv[i]);
			}
		else if ((!strcmp(argv[i],"--smooth")) && (i < argc-1))
			{
			options->filterSmooth = atoi(argv[++i]);
			}
		else if ((!strcmp(argv[i],"--lock")) && (i < argc-1))
			{
			options->filterLock = atof(argv[++i]);
			}
		else if ((!strcmp(argv[i],"--help")) || (!strcmp(argv[i],"-h")))
			fprintf(stderr,"Usage: %s [options]\n"
				"Options:\n"
#ifdef __sgi
				"\t--port SERIAL_PORT		(default is /dev/ttyd2)\n"
#else
				"\t--port SERIAL_PORT		(default is /dev/ttyS0)\n"
#endif
				"\t--shmem SHARED_MEMORY_KEY	(default is 9900)\n"
				"\t--offset X Y Z			(default is 0 0 0)\n"
				"\t--xrot X_ROTATION		(default is 0)\n"
				"\t--mode retro|stereo|stereo60	(default is retro)\n"
				"\t--smooth NUM_READINGS	(default is no smoothing)\n"
				"\t--lock MIN_DISTANCE		(default is 0)\n"
				"\t--othertracker SHARED_MEMORY_KEY	(default is no other tracker)\n"
				,argv[0]);
		else
			fprintf(stderr,"WARNING: unknown option \"%s\"\n",
				argv[i]);
		}
	options->xRotCos = cos(options->xRot * M_PI / 180.0);
	options->xRotSin = sin(options->xRot * M_PI / 180.0);
	}


void dyna_init_tracker(struct _dyna_options *options)
	{
	struct termio port_attributes;

	dyna_fd = open(options->portname,O_RDWR | O_NOCTTY | O_NDELAY);
	if (dyna_fd == -1)
		{
		fprintf(stderr,"Serial port can not be opened\n");
		perror(options->portname);
		exit(-1);
		}

	/* get port attributes */
	ioctl(dyna_fd,TCGETA,&port_attributes);
	/* set new port attributes */
	port_attributes.c_iflag  = 0;
	port_attributes.c_oflag  = 0;
	port_attributes.c_lflag  = 0;
	port_attributes.c_line  = 0;
#ifdef __sgi
/* This code is specifically for IRIX 6.5, where the termio struct changed.
   Earlier versions of IRIX should use the code in the #else clause.  */
	port_attributes.c_cflag = (CS8 | CLOCAL | CREAD);
	port_attributes.c_ispeed = 19200;
	port_attributes.c_ospeed = 19200;
#else
	port_attributes.c_cflag = (B19200 | CS8 | CLOCAL | CREAD);
#endif
	port_attributes.c_cc[0] = 0; 
	port_attributes.c_cc[1] = 0; 
	port_attributes.c_cc[2] = 0; 
	port_attributes.c_cc[3] = 0; 
	port_attributes.c_cc[4] = 0; 
	port_attributes.c_cc[5] = 100;
	ioctl(dyna_fd,TCSETA,&port_attributes);
	 dyna_reset_tracker(options);
	}


#define DYNABUFSIZE 32
static unsigned char dyna_buf[DYNABUFSIZE];
static int dyna_bytes = 0;

void dyna_process_byte(unsigned char c)
	{
	dyna_buf[dyna_bytes++] = c;
	if (dyna_bytes < 3)
 		{
		if ((c & 0xf0) != 0x80)
			dyna_bytes = 0;
		}
	else if (dyna_bytes == 3)
	 	{
		if ((c & 0xf0) == 0x80)
			{
			dyna_buf[0] = dyna_buf[1];
			dyna_buf[1] = dyna_buf[2];
			dyna_bytes = 2;
			}
		}
	if (dyna_bytes >= DYNABUFSIZE)
		{
		fprintf(stderr,"ERROR: Overflow of input buffer "
			"(this should never even happen!)\n");
		dyna_bytes = 0;
		}
	}


/* According to the manual, the reported x/y/z are "scaled at .05mm
   per least significant bit".  .05 mm is .000164042 feet */
#define DYNASIGHT_RESOLUTION 0.000164042


int dyna_read_data(CAVE_SENSOR_ST *sensor,struct _dyna_options *options)
	{
	unsigned char c=0;
	while ((read(dyna_fd,&c,1) > 0) && (dyna_bytes < 8))
		dyna_process_byte(c);
	if (dyna_bytes >= 8)
		{
		int exponent;
		signed short x, y, z;
		float newY, newZ;
		dyna_bytes = 0;
		if ((dyna_buf[0] & 0x0c) != 0)
			return 0;
		exponent = dyna_buf[0] & 3;
		x = (dyna_buf[2] << 8) | dyna_buf[3];
		y = (dyna_buf[4] << 8) | dyna_buf[5];
		z = (dyna_buf[6] << 8) | dyna_buf[7];
		sensor->x = x * (DYNASIGHT_RESOLUTION * (1 << exponent));
		sensor->y = y * (DYNASIGHT_RESOLUTION * (1 << exponent));
		sensor->z = z * (DYNASIGHT_RESOLUTION * (1 << exponent));
		sensor->azim = 0;
		sensor->elev = 0;
		sensor->roll = 0;
		get_timestamp((uint32_t*)&sensor->timestamp);
		newY = sensor->y * options->xRotCos -
			sensor->z * options->xRotSin;
		newZ = sensor->y * options->xRotSin +
			sensor->z * options->xRotCos;
		sensor->x += options->offset[0];
		sensor->y = newY + options->offset[1];
		sensor->z = newZ + options->offset[2];
		if (dyna_filter_reading(sensor,options))
			return 1;
		}
	return 0;
	}



int dyna_filter_reading(CAVE_SENSOR_ST *sensor,struct _dyna_options *options)
	{
	static CAVE_SENSOR_ST prev, *smoothBuf;
	static int bufIndex = 0, smoothCount = 0;
	static int firstTime = 1;
	if (firstTime)
		{
		prev = *sensor;
		if (options->filterSmooth > 0)
			smoothBuf = (CAVE_SENSOR_ST *) malloc(options->filterSmooth * sizeof(CAVE_SENSOR_ST));
		firstTime = 0;
		return 1;
		}
	if (options->filterLock > 0)
		{
		float dx,dy,dz;
		dx = sensor->x - prev.x;
		dy = sensor->y - prev.y;
		dz = sensor->z - prev.z;
		if ((dx*dx+dy*dy+dz*dz) > (options->filterLock*options->filterLock))
			prev = *sensor;
		else
			return 0;
		}
	if (options->filterSmooth > 0)
		{
		smoothBuf[bufIndex] = *sensor;
		bufIndex = (bufIndex + 1) % options->filterSmooth;
		if (smoothCount >= options->filterSmooth)
			{
			int i;
			CAVE_SENSOR_ST sum;
			sum.x = sum.y = sum.z = 0;
			for (i=0; i < options->filterSmooth; i++)
				{
				sum.x += smoothBuf[i].x;
				sum.y += smoothBuf[i].y;
				sum.z += smoothBuf[i].z;
				}
			sensor->x = sum.x / options->filterSmooth;
			sensor->y = sum.y / options->filterSmooth;
			sensor->z = sum.z / options->filterSmooth;
			}
		else
			smoothCount++;
		}
	return 1;
	}


void dyna_handle_command(int cmd,struct _dyna_options *options)
	{
	if (cmd == CAVE_TRACKD_RESET_COMMAND)
		dyna_reset_tracker(options);
	}


void dyna_reset_tracker(struct _dyna_options *options)
	{
	/* Set tracker to 'retro' mode */
	write(dyna_fd,"\003",1);
	if (options->mode == DYNA_RETRO)
		write(dyna_fd,"0",1);
	else if (options->mode == DYNA_STEREO)
		write(dyna_fd,"2",1);
	else if (options->mode == DYNA_STEREO60)
		write(dyna_fd,"V",1);
	}



/******************************************************************/


#define PERMS 0666

static int tracker_shmid=-1;


/*************************************************************************
 void trackd_cleanup()
	When the daemon is interrupted by SIGINT, this routine closes the
 connections, disposes of the memory, and exits.
**************************************************************************/
void trackd_cleanup(int foo)
{
 fprintf(stderr,"Interrupted.  Quitting...\n");
 if (tracker)
	if (shmdt(tracker) < 0)
		perror("Detaching 'tracker' shared memory");
 if (tracker_shmid > -1)
	if (shmctl(tracker_shmid,IPC_RMID,NULL) < 0)
		perror("Removing 'tracker_shmid'");
 exit(-1);
}

/*************************************************************************
 void get_tracker_mem()
	Allocates shared memory for the sensors.  The global 'tracker'
  points to the chunk.
**************************************************************************/
void get_tracker_mem(int shmemKey)
{
 tracker_shmid = shmget(shmemKey, sizeof(struct TRACKD_TRACKING),
			PERMS | IPC_CREAT);
 if (tracker_shmid < 0)
	{
	fprintf(stderr,"can't get shared memory\n");
	exit(-1);
	}
 tracker = (struct TRACKD_TRACKING *) shmat(tracker_shmid,(char *) 0, 0);
 if (tracker == (struct TRACKD_TRACKING *) -1)
	{
	fprintf(stderr,"can't attach shared memory\n");
	exit(-1);
	}
}

		
/*************************************************************************
 void init_tracker_data()
	Fills in the header of the tracker data, and fills all the sensors
  with defaults (0 0 0).
**************************************************************************/
void init_tracker_data(void)
{
 int loop;
 CAVE_SENSOR_ST sensor;

 tracker->header.version = CAVELIB_2_6;
 tracker->header.numSensors = 1;
 tracker->header.sensorOffset = ((char *)&tracker->sensor[0]) - ((char *)tracker);
 tracker->header.sensorSize = sizeof(CAVE_SENSOR_ST);
 tracker->header.timestamp[0] = tracker->header.timestamp[1] = 0;
 tracker->header.command = 0;

 sensor.x = 0;
 sensor.y = 0;
 sensor.z = 0;
 sensor.elev = 0;
 sensor.azim = 0;
 sensor.roll = 0;
 sensor.calibrated = 0;
 sensor.frame = CAVE_TRACKER_FRAME;

 for (loop=0; loop < TRACKD_MAX_SENSORS; ++loop)
	tracker->sensor[loop] = sensor;
}


void get_timestamp(uint32_t stamp[2])
{
 struct timeval curtime;
 gettimeofday(&curtime,NULL);
 stamp[0] = curtime.tv_sec;
 stamp[1] = curtime.tv_usec;
}


/*************************************************************************/


struct TRACKD_TRACKING *otherTracker;

void attach_other_tracker_mem(int otherTrackerKey)
	{
	int shmemId;
	shmemId = shmget(otherTrackerKey,sizeof(struct TRACKD_TRACKING),0);
	if (shmemId < 0)
		{
		fprintf(stderr,"ERROR: couldn't get other trackd's memory"
			" (key=%d)\n",otherTrackerKey);
		perror("shmget");
		return;
		}
	otherTracker = (struct TRACKD_TRACKING *) shmat(shmemId,(void *) 0, 0);
 	if (otherTracker == (struct TRACKD_TRACKING *) -1)
		{
		fprintf(stderr,"ERROR: couldn't attach to other trackd's memory"
			" (key=%d)\n",otherTrackerKey);
		perror("shmat");
		otherTracker = NULL;
		}
	tracker->header.numSensors = otherTracker->header.numSensors;
	}


/** NOTE: This function is sloppy, in that it makes two major assumptions:
	1. the other tracker updates its header's timestamp whenever there's
		new data
	2. the other tracker is using the same shared memory format (i.e. it's
		also a "CAVELIB_2_6" trackd
    If this daemon is going to be used in production, the second assumption
    should definitely be fixed by using the proper general-purpose trackd
    data-access methods.  The first could be gotten around by checking each
    sensor individually.
**/
void check_other_tracker(struct TRACKD_TRACKING	*tracker)
	{
	static uint32_t lastTime[2] = {0,0};
	if (!otherTracker)
		return;
	if ((otherTracker->header.timestamp[0] != lastTime[0]) ||
	    (otherTracker->header.timestamp[1] != lastTime[1]))
		{
		int i;
		lastTime[0] = otherTracker->header.timestamp[0];
		lastTime[1] = otherTracker->header.timestamp[1];
		for (i=1; (i < TRACKD_MAX_SENSORS) &&
			  (i < otherTracker->header.numSensors); ++i)
			{
			tracker->sensor[i] = otherTracker->sensor[i];
			}
		get_timestamp(tracker->header.timestamp);
		}
	}