#import "PhidgetGyroscopeController.h"
#include <time.h>
#include <sys/time.h>

CPhidgetGyroscopeHandle gyro;

double zaxis[1000], xaxis[1000], yaxis[1000];
int xcount, ycount, zcount;
time_t start;
struct timeval xtime, ytime, ztime;
double xposn, yposn, zposn;

@implementation PhidgetGyroscopeController

int gotAttach(CPhidgetHandle phid, void *context) {
	[(id)context phidgetAdded];
	return 0;
}

int gotDetach(CPhidgetHandle phid, void *context) {
	[(id)context phidgetRemoved];
	return 0;
}

int gotGyro(CPhidgetGyroscopeHandle phid, void *context, int ind, double val) {
	[(id)context gotGyroChange:ind:val];
	return 0;
}

- (void)gotGyroChange:(int)Index:(double)Value
{
	//printf("Gyro change: %lf\n",Value); return;
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	
	//if(Value > 1 || Value < -1)
	//	printf("Change > 1: Axis: %d Rate: %lf\n",Index, Value);
	
	int i=0;
	double out = 0;
	struct timeval now3;
	gettimeofday(&now3,NULL);
	
	double offset;
	time_t now, now2;
	
	now = clock();
	now2 = time(0);
	
	if(Index == 0) {
		if(xcount>0)
		{
			out = 0;
			for(i=0;i<xcount;i++)
			{
				out+=xaxis[i];
			}

			offset = (now3.tv_sec - xtime.tv_sec) + (double)((now3.tv_usec-xtime.tv_usec)/1000000.0);
			xposn += (out/xcount)*offset;
			xtime.tv_sec = now3.tv_sec;
			xtime.tv_usec = now3.tv_usec;
			
			xcount = 0;
			[customView setX:xposn];
			[customView setNeedsDisplay:YES];
			
			[xrate setTitleWithMnemonic:[NSString stringWithFormat:@"%3.4lf",xposn]];
		}
		else
		{
			xaxis[xcount++] = Value;
		}
	}
	else if(Index == 1){
		if(ycount>0)
		{
			out = 0;
			for(i=0;i<ycount;i++)
			{
				out+=yaxis[i];
			}
			
			offset = (now3.tv_sec - ytime.tv_sec) + (double)((now3.tv_usec-ytime.tv_usec)/1000000.0);
			yposn += (out/ycount)*offset;
			ytime.tv_sec = now3.tv_sec;
			ytime.tv_usec = now3.tv_usec;
			
			ycount = 0;
			[customView setY:yposn];
			[customView setNeedsDisplay:YES];
			
			[yrate setTitleWithMnemonic:[NSString stringWithFormat:@"%3.4lf",yposn]];
		}
		else
		{
			yaxis[ycount++] = Value;
		}
	}
	else {
		if(zcount>0)
		{
			out = 0;
			for(i=0;i<zcount;i++)
			{
				out+=zaxis[i];
			}
			
			offset = (now3.tv_sec - ztime.tv_sec) + (double)((now3.tv_usec-ztime.tv_usec)/1000000.0);
			zposn += (out/zcount)*offset;
			ztime.tv_sec = now3.tv_sec;
			ztime.tv_usec = now3.tv_usec;
			
			zcount = 0;
			[customView setZ:zposn];
			[customView setNeedsDisplay:YES];
			
			[zrate setTitleWithMnemonic:[NSString stringWithFormat:@"%3.4lf",zposn]];
		}
		else
		{
			zaxis[zcount++] = Value;
			
		}
	}
	
	time_t elapsed = now2-start;
	[timeField setTitleWithMnemonic:[NSString stringWithFormat:@"%02d:%02d:%02d",elapsed/3600,(elapsed%3600)/60,elapsed%60]];
	
	//printf("Positions: x:%3.4lf y:%3.4lf z:%3.4lf\n",xposn, yposn, zposn);
	
	[pool release];
}

- (void)phidgetAdded
{
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	
	int serial, version;
	int numGyros;
	
	CPhidget_getSerialNumber((CPhidgetHandle)gyro, &serial);
	CPhidget_getDeviceVersion((CPhidgetHandle)gyro, &version);
	CPhidgetGyroscope_getAxisCount(gyro,&numGyros);
	
	[connectedField setTitleWithMnemonic:@"PhidgetGyroscope Connected"];
	[serialNumberField setTitleWithMnemonic:[NSString stringWithFormat:@"%d", serial]];
	[versionField setTitleWithMnemonic:[NSString stringWithFormat:@"%d", version]];
	[numberOfAxesField setTitleWithMnemonic:[NSString stringWithFormat:@"%i", numGyros]];
	
	start = time(0);
	gettimeofday(&xtime,NULL);
	gettimeofday(&ytime,NULL);
	gettimeofday(&ztime,NULL);
	xposn=0;
	yposn=0;
	zposn=0;
	xcount=0;
	ycount=0;
	zcount=0;
			
	[pool release];
}

- (void)phidgetRemoved
{
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	
	[connectedField setTitleWithMnemonic:@"PhidgetGyroscope Removed"];
	[serialNumberField setTitleWithMnemonic:@""];
	[versionField setTitleWithMnemonic:@""];
	[numberOfAxesField setTitleWithMnemonic:@""];
			
	[pool release];
}

/*
* This gets run when the GUI gets displayed
*/
- (void)awakeFromNib
{
	int serial = -1;
	NSArray *args = [[NSProcessInfo processInfo] arguments];
	if([args count] > 1)
	{
		NSString *serialString = [args objectAtIndex:1];
		serial = strtol([serialString UTF8String], NULL, 10);
		if(serial == 0) serial = -1;
	}
	
	[mainWindow setDelegate:self];
	
	CPhidget_enableLogging(PHIDGET_LOG_INFO, NULL);
	CPhidgetGyroscope_create(&gyro);
	
	CPhidgetGyroscope_set_OnAngularRateChange_Handler(gyro, &gotGyro, self);
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)gyro, gotAttach, self);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)gyro, gotDetach, self);
	
	CPhidget_open((CPhidgetHandle)gyro, serial);
}

- (void)windowWillClose:(NSNotification *)aNotification {
	CPhidget_close((CPhidgetHandle)gyro);
	CPhidget_delete((CPhidgetHandle)gyro);
	gyro = NULL;
	[NSApp terminate:self];
}

@end
