//
//  PhidgetSpatialController.m
//  CocoaSpatial
//
//  Created by Patrick McNeil on 10-05-20.
//  Copyright 2010 Phidgets Inc. All rights reserved.
//

#import "PhidgetSpatialController.h"

//Define data rate to be 32ms
#define dataRate 32

int getmag(const char *mdfile, double longitude, double latitude, double alt, double *dres, double *fres);

//Event callback functions for C, which in turn call a method on the GUI object in it's thread context
int gotAttach(CPhidgetHandle phid, void *context) {
	[(id)context performSelectorOnMainThread:@selector(phidgetAdded:)
								  withObject:nil
							   waitUntilDone:NO];
	return 0;
}
int gotDetach(CPhidgetHandle phid, void *context) {
	[(id)context performSelectorOnMainThread:@selector(phidgetRemoved:)
								  withObject:nil
							   waitUntilDone:NO];
	return 0;
}
int gotSpatialData(CPhidgetSpatialHandle phid, void *context, CPhidgetSpatial_SpatialEventDataHandle *data, int dataCount) {
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	//We just pass the first set of Spatial data - there could be up to 8 sets
	[(id)context performSelectorOnMainThread:@selector(SpatialData:)
								  withObject:[[PhidgetSpatialEventData alloc] initWithHandle:data[0]]
							   waitUntilDone:NO];
	[pool release];
	return 0;
}

@implementation PhidgetSpatialController

- (void)setPicture:(int)version:(CPhidget_DeviceID)devid
{
	NSImage *img = nil;
	
	switch(devid)
	{
		case PHIDID_SPATIAL_ACCEL_3AXIS:
			img = [NSImage imageNamed:@"1049_0"];
			break;
		case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
			img = [NSImage imageNamed:@"1056_0"];
			break;
		default:
			break;
	}
	
	//don't set the dock icon to nil
	if(img)
		[NSApp setApplicationIconImage:img];
	
	//ok to set the picture to nil
	[pictureBox setImage:img];
}

- (void)phidgetAdded:(id)nothing
{
	int serial, version;
	const char *name;
	CPhidget_DeviceID devid;
	NSRect frame = [mainWindow frame];
	
	CPhidget_getSerialNumber((CPhidgetHandle)spatial, &serial);
	CPhidget_getDeviceVersion((CPhidgetHandle)spatial, &version);
	CPhidget_getDeviceName((CPhidgetHandle)spatial, &name);
	CPhidget_getDeviceID((CPhidgetHandle)spatial, &devid);
	CPhidgetSpatial_getAccelerationAxisCount(spatial, &numAccelAxes);
	CPhidgetSpatial_getGyroAxisCount(spatial, &numGyroAxes);
	CPhidgetSpatial_getCompassAxisCount(spatial, &numCompassAxes);
	
	[connectedField setStringValue:[NSString stringWithCString:name encoding:NSASCIIStringEncoding]];
	[serialField setIntValue:serial];
	[versionField setIntValue:version];
	[numAccelAxesField setIntValue:numAccelAxes];
	[numGyroAxesField setIntValue:numGyroAxes];
	[numCompassAxesField setIntValue:numCompassAxes];
	
	//filter the data a bit so it looks nicer
	CPhidgetSpatial_setDataRate(spatial, dataRate);
	accelFilter = [[LowpassFilter alloc] initWithSampleRate:1000.0/dataRate cutoffFrequency:7.5];
	magFieldFilter = [[LowpassFilter alloc] initWithSampleRate:1000.0/dataRate cutoffFrequency:7.5];
	compassBearing = [[CompassBearing alloc] init];
	[compassBearing setSampleRate:1000.0/dataRate cutoffFrequency:15.0];
	
	if([[northTypeButtons cellAtRow:0 column:1] state] == true)
		[compassBearing setUseDeclination:YES];
	
	int heightChange;
	int widthChange;
	switch(devid)
	{
		case PHIDID_SPATIAL_ACCEL_3AXIS:
			[accelBox setHidden:FALSE];
			heightChange = frame.size.height - 492;
			widthChange = frame.size.width - 317;
			break;
		case PHIDID_SPATIAL_ACCEL_GYRO_COMPASS:
			[accelBox setHidden:FALSE];
			[gyroBox setHidden:FALSE];
			[magFieldBox setHidden:FALSE];
			[compassBox setHidden:FALSE];
			[magFieldView setCircleRadius:magFieldStrength];
			heightChange = frame.size.height - 781;
			widthChange = frame.size.width - 602;
			break;
		default:
			break;
	}
	
	frame.origin.y += heightChange;
	frame.size.height -= heightChange;
	frame.size.width -= widthChange;
	[mainWindow setMinSize:frame.size];
	[mainWindow setFrame:frame display:YES animate:NO];
	
	[self setPicture:version:devid];
}

- (void)phidgetRemoved:(id)nothing
{
	[connectedField setStringValue:@"Nothing"];
	[serialField setStringValue:@""];
	[versionField setStringValue:@""];
	[numAccelAxesField setStringValue:@""];
	[numGyroAxesField setStringValue:@""];
	[numCompassAxesField setStringValue:@""];
	
	[pictureBox setImage:nil];
	
	[accelFilter release];
	[magFieldFilter release];
	[compassBearing release];
	
	[accelBox setHidden:TRUE];
	[gyroBox setHidden:TRUE];
	[magFieldBox setHidden:TRUE];
	[compassBox setHidden:TRUE];
	
	NSRect frame = [mainWindow frame];
	int heightChange = frame.size.height - 203;
	int widthChange = frame.size.width - 317;
	frame.origin.y += heightChange;
	frame.size.height -= heightChange;
	frame.size.width -= widthChange;
	[mainWindow setMinSize:frame.size];
	[mainWindow setFrame:frame display:YES animate:NO];
}

- (IBAction)zeroGyro:(id)sender
{
	CPhidgetSpatial_zeroGyro(spatial);
	gyroHeading[0]=gyroHeading[1]=gyroHeading[2] = 0;
}

- (void)calculateGyroHeadings:(PhidgetSpatialEventData *)data
{
	double gyro = 0;
	for (int i = 0; i < numGyroAxes; i++)
	{
		double timestampSeconds = [data Data]->timestamp.seconds + [data Data]->timestamp.microseconds/1000000.0;
		gyro = [data Data]->angularRate[i];
		
		if (lastMsCountGood[i] == true)
		{
			//calculate heading
			double timechange = timestampSeconds - lastMsCount[i];
			
			gyroHeading[i] += timechange * gyro;
		}
		
		lastMsCount[i] = timestampSeconds;
		lastMsCountGood[i] = true;
	}
}

- (void)SpatialData:(PhidgetSpatialEventData *)spatialData
{
	if(numAccelAxes > 0)
	{
		[accelFilter addSpatialVector:[spatialData Data]->acceleration[0] :[spatialData Data]->acceleration[1] :[spatialData Data]->acceleration[2]];
		if([accelFilterEnabled state])
		{
			[accelView setVector:[accelFilter x]: [accelFilter y]: [accelFilter z]];
		}
		else
		{
			[accelView setVector:[spatialData Data]->acceleration[0] :[spatialData Data]->acceleration[1] :[spatialData Data]->acceleration[2]];
		}
	}
	
	//Need to make sure that the magnetic field data exists, it will be PUNK_DBL briefly every two seconds
	if(numCompassAxes > 0 && [spatialData Data]->magneticField[0] != PUNK_DBL)
	{
		[magFieldFilter addSpatialVector:[spatialData Data]->magneticField[0] :[spatialData Data]->magneticField[1] :[spatialData Data]->magneticField[2]];
		if([magFieldFilterEnabled state])
		{
			[magFieldView setVector:[magFieldFilter x] :[magFieldFilter y] :[magFieldFilter z]];
		}
		else
		{
			[magFieldView setVector:[spatialData Data]->magneticField[0] :[spatialData Data]->magneticField[1] :[spatialData Data]->magneticField[2]];
		}
		[compassBearing setAccelerationVector:[spatialData Data]->acceleration[0] :[spatialData Data]->acceleration[1] :[spatialData Data]->acceleration[2]];
		[compassBearing setMagneticFieldVector:[spatialData Data]->magneticField[0] :[spatialData Data]->magneticField[1] :[spatialData Data]->magneticField[2]];
		[compassBearing setAccelerationVector:[accelFilter x]: [accelFilter y]: [accelFilter z]];
		[compassBearing setMagneticFieldVector:[magFieldFilter x] :[magFieldFilter y] :[magFieldFilter z]];
		[compassBearing setDeclination:magDeclination];
		[compassBearing updateBearing];
		
		[bearingField setStringValue:[NSString stringWithFormat:@"%0.1lf°",[compassBearing bearingDegrees]]];
		[compassView setBearing:[compassBearing bearing]];
	}
	
	if(numGyroAxes > 0)
	{
		[self calculateGyroHeadings: spatialData];
		[gyroView setHeadings:gyroHeading[0] :gyroHeading[1] :gyroHeading[2]];
	}

}

/*
 * This gets run when the GUI gets displayed
 */
- (void)awakeFromNib
{
	int serial = -1, remote = 0;
	NSArray *args = [[NSProcessInfo processInfo] arguments];
	if([args count] > 1)
	{
		if([[args objectAtIndex:1] isEqualToString:@"remote"])
			remote = 1;
		serial = [[args objectAtIndex:[args count]-1] intValue];
		if(serial == 0) serial = -1;
	}
	
	[mainWindow setDelegate:self];
	
	magFieldStrength = 0.5; //default
	magDeclination = 0.0; //default
	
	CPhidgetSpatial_create(&spatial);
	
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)spatial, gotAttach, self);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)spatial, gotDetach, self);
	CPhidgetSpatial_set_OnSpatialData_Handler(spatial, gotSpatialData, self);
	
	if(remote)
		CPhidget_openRemote((CPhidgetHandle)spatial, serial, NULL, NULL);
	else
		CPhidget_open((CPhidgetHandle)spatial, serial);
}

- (void)windowWillClose:(NSNotification *)aNotification {
	
	//Because this event is called twice
	if(spatial)
	{
		//Stop all events before closing
		CPhidget_set_OnAttach_Handler((CPhidgetHandle)spatial, NULL, NULL);
		CPhidget_set_OnDetach_Handler((CPhidgetHandle)spatial, NULL, NULL);
		CPhidgetSpatial_set_OnSpatialData_Handler(spatial, NULL, NULL);
		
		CPhidget_close((CPhidgetHandle)spatial);
		CPhidget_delete((CPhidgetHandle)spatial);
		spatial = NULL;
	}
	[NSApp terminate:self];
}

@end
