#import "PhidgetMotorController.h"

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 gotInputChange(CPhidgetMotorControlHandle phid, void *context, int ind, int val) {
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	[(id)context performSelectorOnMainThread:@selector(InputChange:)
								  withObject:[NSArray arrayWithObjects:[NSNumber numberWithInt:ind], [NSNumber numberWithInt:val], nil]
							   waitUntilDone:NO];
	[pool release];
	return 0;
}

int gotVelocityChange(CPhidgetMotorControlHandle phid, void *context, int ind, double val) {
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	[(id)context performSelectorOnMainThread:@selector(VelocityChange:)
								  withObject:[NSArray arrayWithObjects:[NSNumber numberWithInt:ind], [NSNumber numberWithDouble:val], nil]
							   waitUntilDone:NO];
	[pool release];
	return 0;
}

int gotCurrentChange(CPhidgetMotorControlHandle phid, void *context, int ind, double val) {
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	[(id)context performSelectorOnMainThread:@selector(CurrentChange:)
								  withObject:[NSArray arrayWithObjects:[NSNumber numberWithInt:ind], [NSNumber numberWithDouble:val], nil]
							   waitUntilDone:NO];
	[pool release];
	return 0;
}

int gotBackEMFUpdate(CPhidgetMotorControlHandle phid, void *context, int ind, double val) {
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	[(id)context performSelectorOnMainThread:@selector(BackEMFUpdate:)
								  withObject:[NSArray arrayWithObjects:[NSNumber numberWithInt:ind], [NSNumber numberWithDouble:val], nil]
							   waitUntilDone:NO];
	[pool release];
	return 0;
}

int gotSensorUpdate(CPhidgetMotorControlHandle phid, void *context, int ind, int val) {
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	[(id)context performSelectorOnMainThread:@selector(SensorUpdate:)
								  withObject:[NSArray arrayWithObjects:[NSNumber numberWithInt:ind], [NSNumber numberWithInt:val], nil]
							   waitUntilDone:NO];
	[pool release];
	return 0;
}

int gotEncoderPositionChange(CPhidgetMotorControlHandle phid, void *context, int ind, int time, int positionChange) {
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	[(id)context performSelectorOnMainThread:@selector(EncoderPositionChange:)
								  withObject:[NSArray arrayWithObjects:[NSNumber numberWithInt:ind], [NSNumber numberWithInt:time], [NSNumber numberWithInt:positionChange], nil]
							   waitUntilDone:NO];
	[pool release];
	return 0;
}

@implementation PhidgetMotorController

- (IBAction)setAccel:(id)sender
{
	double accel = [[sender cellWithTag:[sender selectedTag]] doubleValue];
	CPhidgetMotorControl_setAcceleration(motor, [sender selectedTag], accel);
	[[accelerationFields cellWithTag:[sender selectedTag]] setStringValue:[NSString stringWithFormat:@"%0.1f", accel]];
}

- (IBAction)setVelocity:(id)sender
{
	double velocity = [[sender cellWithTag:[sender selectedTag]] doubleValue];
	CPhidgetMotorControl_setVelocity(motor, [sender selectedTag], velocity);
	[[targetVelocityFields cellWithTag:[sender selectedTag]] setStringValue:[NSString stringWithFormat:@"%0.1f", velocity]];
}

- (IBAction)setBackEMFState:(id)sender
{
	int state = [[sender cellWithTag:[sender selectedTag]] intValue];
	CPhidgetMotorControl_setBackEMFSensingState(motor, [sender selectedTag], state);
}

- (IBAction)setBraking:(id)sender
{
	double braking = [[sender cellWithTag:[sender selectedTag]] doubleValue];
	CPhidgetMotorControl_setBraking(motor, [sender selectedTag], braking);
	[[brakingFields cellWithTag:[sender selectedTag]] setStringValue:[NSString stringWithFormat:@"%0.1f", braking]];
}

- (IBAction)resetEncoderPosition:(id)sender
{
	int posn;
	CPhidgetMotorControl_setEncoderPosition(motor, 0, 0);
	CPhidgetMotorControl_getEncoderPosition(motor, 0, &posn);
	[encoderPosition setIntValue:posn];
	[encoderPositionSlider setIntValue:posn];
}

- (IBAction)setRatiometric:(id)sender
{
	CPhidgetMotorControl_setRatiometric(motor, [sender state]);
}

- (void)resizeMatrix:(NSMatrix *)matrix:(int)size
{
	[matrix renewRows:size columns:1];
	NSSize size1 = [matrix frame].size;
	size1.height = 19*size;
	[matrix setFrameSize:size1];
}

- (void)phidgetAdded:(id)nothing
{
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	
	int serial, version, i;
	const char *name;
	CPhidget_DeviceID devid;
	int heightChange, widthChange;
	
	CPhidget_getSerialNumber((CPhidgetHandle)motor, &serial);
	CPhidget_getDeviceVersion((CPhidgetHandle)motor, &version);
	CPhidget_getDeviceName((CPhidgetHandle)motor, &name);
	CPhidget_getDeviceID((CPhidgetHandle)motor, &devid);
	CPhidgetMotorControl_getInputCount(motor, &numInputs);
	CPhidgetMotorControl_getMotorCount(motor, &numMotors);
	CPhidgetMotorControl_getEncoderCount(motor, &numEncoders);
	CPhidgetMotorControl_getSensorCount(motor, &numSensors);
	
	[connectedField setStringValue:[NSString stringWithCString:name]];
	[serialField setIntValue:serial];
	[versionField setIntValue:version];
	[numInputsField setIntValue:numInputs];
	[numMotorsField setIntValue:numMotors];
	[numEncodersField setIntValue:numEncoders];
	[numSensorsField setIntValue:numSensors];
	
	[self resizeMatrix:motorControlLabels :numMotors];
	[self resizeMatrix:targetVelocitySliders :numMotors];
	[self resizeMatrix:targetVelocityFields :numMotors];
	[self resizeMatrix:accelerationSliders :numMotors];
	[self resizeMatrix:accelerationFields :numMotors];
	[self resizeMatrix:brakingSliders :numMotors];
	[self resizeMatrix:brakingFields :numMotors];
	[self resizeMatrix:backEMFStateChecks :numMotors];
	[self resizeMatrix:motorStateLabels :numMotors];
	[self resizeMatrix:velocitySliders :numMotors];
	[self resizeMatrix:velocityFields :numMotors];
	[self resizeMatrix:currentSliders :numMotors];
	[self resizeMatrix:currentFields :numMotors];
	[self resizeMatrix:backEMFSliders :numMotors];
	[self resizeMatrix:backEMFFields :numMotors];
	
	for(i=0;i<numMotors;i++)
	{
		double maxAccel, minAccel, acceleration, velocity, braking, backEMF;
		int backEMFState;
		
		CPhidgetMotorControl_getAccelerationMax(motor, i, &maxAccel);
		CPhidgetMotorControl_getAccelerationMin(motor, i, &minAccel);
		[[accelerationSliders cellWithTag:i] setMinValue:minAccel];
		[[accelerationSliders cellWithTag:i] setMaxValue:maxAccel];
		
		if(!CPhidgetMotorControl_getAcceleration(motor, i, &acceleration))
		{
			[[accelerationFields cellWithTag:i] setStringValue:[NSString stringWithFormat:@"%0.1f", acceleration]];
			[[accelerationSliders cellWithTag:i] setDoubleValue:acceleration];
		}
		else {
			[[accelerationFields cellWithTag:i] setStringValue:@""];
			[[accelerationSliders cellWithTag:i] setDoubleValue:minAccel];
		}

		if(!CPhidgetMotorControl_getVelocity(motor, i, &velocity))
		{
			[[targetVelocityFields cellWithTag:i] setStringValue:[NSString stringWithFormat:@"%0.1f", velocity]];
			[[targetVelocitySliders cellWithTag:i] setDoubleValue:velocity];
		}
		else {
			[[targetVelocityFields cellWithTag:i] setStringValue:@""];
			[[targetVelocitySliders cellWithTag:i] setDoubleValue:0];
		}
		
		if(!CPhidgetMotorControl_getBackEMFSensingState(motor, i, &backEMFState))
		{
			[[backEMFStateChecks cellWithTag:i] setState:backEMFState];
		}
		else
		{
			[[backEMFStateChecks cellWithTag:i] setState:0];
		}
		
		if(!CPhidgetMotorControl_getBraking(motor, i, &braking))
		{
			[[brakingFields cellWithTag:i] setStringValue:[NSString stringWithFormat:@"%0.1f", braking]];
			[[brakingSliders cellWithTag:i] setDoubleValue:braking];
		}
		else {
			[[brakingFields cellWithTag:i] setStringValue:@""];
			[[brakingSliders cellWithTag:i] setDoubleValue:0];
		}
		
		if(!CPhidgetMotorControl_getBackEMF(motor, i, &backEMF))
		{
			[[backEMFFields cellWithTag:i] setStringValue:[NSString stringWithFormat:@"%0.3f", backEMF]];
			[[backEMFSliders cellWithTag:i] setDoubleValue:backEMF];
		}
		else {
			[[backEMFFields cellWithTag:i] setStringValue:@""];
			[[brakingSliders cellWithTag:i] setDoubleValue:0];
		}
	}
	
	if(numEncoders > 0)
	{
		int posn;
		CPhidgetMotorControl_getEncoderPosition(motor, 0, &posn);
		[encoderPosition setIntValue:posn];
		[encoderPositionSlider setIntValue:posn];
	}
	
	if(numSensors > 0)
	{
		int ratioState;
		CPhidgetMotorControl_getRatiometric(motor, &ratioState);
		[ratiometricCheck setState:ratioState];
	}
	
	NSRect frame = [mainWindow frame];
	
	switch(devid)
	{
		case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT:
			heightChange = frame.size.height - 535;
			widthChange = frame.size.width - 431;
			
			[inputBox setHidden:FALSE];
			
			break;
		case PHIDID_MOTORCONTROL_HC_2MOTOR:
			heightChange = frame.size.height - 473;
			widthChange = frame.size.width - 431;
			
			[currentLabel setHidden:FALSE];
			[currentSliders setHidden:FALSE];
			[currentFields setHidden:FALSE];
			
			for(i=0;i<numMotors;i++)
			{
				[[currentSliders cellWithTag:i] setMaxValue:32];
			}
			break;
		case PHIDID_MOTORCONTROL_1MOTOR:
			heightChange = frame.size.height - 593;
			widthChange = frame.size.width - 602;
			
			[brakingLabel setHidden:FALSE];
			[brakingSliders setHidden:FALSE];
			[brakingFields setHidden:FALSE];
			
			[backEMFStateLabel setHidden:FALSE];
			[backEMFStateChecks setHidden:FALSE];
			
			[currentLabel setHidden:FALSE];
			[currentSliders setHidden:FALSE];
			[currentFields setHidden:FALSE];
			
			[backEMFLabel setHidden:FALSE];
			[backEMFSliders setHidden:FALSE];
			[backEMFFields setHidden:FALSE];
			
			[supplyVoltageLabel setHidden:FALSE];
			[supplyVoltageField setHidden:FALSE];
			
			[inputBox setHidden:FALSE];
			[encoderBox setHidden:FALSE];
			[sensorBox setHidden:FALSE];
			
			NSTimer *timer = [NSTimer scheduledTimerWithTimeInterval:0.1
															  target:self
															selector:@selector(updateSupplyVoltage)
															userInfo:nil
															 repeats:YES];
			supplyVoltageTimer = timer;
			
			for(i=0;i<numMotors;i++)
			{
				[[currentSliders cellWithTag:i] setMaxValue:8];
			}
			break;
		default:
			break;
	}
	
	[inputs renewRows:1 columns:numInputs];
	
	[motorsetBox setHidden:FALSE];
	[motorstateBox setHidden:FALSE];
	
	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];
	[pool release];
	[mainWindow display];
}

- (void)phidgetRemoved:(id)nothing
{
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	[connectedField setStringValue:@"Nothing"];
	[serialField setStringValue:@""];
	[versionField setStringValue:@""];
	[numInputsField setStringValue:@""];
	[numMotorsField setStringValue:@""];
	[numEncodersField setStringValue:@""];
	[numSensorsField setStringValue:@""];
	
	[motorsetBox setHidden:TRUE];
	[motorstateBox setHidden:TRUE];
	[inputBox setHidden:TRUE];
	[encoderBox setHidden:TRUE];
	[sensorBox setHidden:TRUE];
	
	[brakingLabel setHidden:TRUE];
	[brakingSliders setHidden:TRUE];
	[brakingFields setHidden:TRUE];
	
	[backEMFStateLabel setHidden:TRUE];
	[backEMFStateChecks setHidden:TRUE];
	
	[currentLabel setHidden:TRUE];
	[currentSliders setHidden:TRUE];
	[currentFields setHidden:TRUE];
	
	[backEMFLabel setHidden:TRUE];
	[backEMFSliders setHidden:TRUE];
	[backEMFFields setHidden:TRUE];
	
	[supplyVoltageLabel setHidden:TRUE];
	[supplyVoltageField setHidden:TRUE];
	
	if(supplyVoltageTimer != nil)
	{
		[supplyVoltageTimer invalidate];
		supplyVoltageTimer = nil;
	}
	
	NSRect frame = [mainWindow frame];
	int heightChange = frame.size.height - 228;
	int widthChange = frame.size.width - 431;
	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:0:0];
	
	[pool release];
	[mainWindow display];
}

- (void)InputChange:(NSArray *)inputData
{
	[[inputs cellWithTag:[[inputData objectAtIndex:0] intValue]] setState:[[inputData objectAtIndex:1] intValue]];
}

- (void)VelocityChange:(NSArray *)velocityData
{
	[[velocitySliders cellWithTag:[[velocityData objectAtIndex:0] intValue]] 
	 setDoubleValue:[[velocityData objectAtIndex:1] doubleValue]];
	[[velocityFields cellWithTag:[[velocityData objectAtIndex:0] intValue]] 
	 setStringValue:[NSString stringWithFormat:@"%0.1lf",[[velocityData objectAtIndex:1] doubleValue]]];
}

- (void)CurrentChange:(NSArray *)currentData
{
	[[currentSliders cellWithTag:[[currentData objectAtIndex:0] intValue]] 
	 setDoubleValue:[[currentData objectAtIndex:1] doubleValue]];
	[[currentFields cellWithTag:[[currentData objectAtIndex:0] intValue]] 
	 setStringValue:[NSString stringWithFormat:@"%0.3lf",[[currentData objectAtIndex:1] doubleValue]]];
}

- (void)BackEMFUpdate:(NSArray *)backEMFData
{
	[[backEMFSliders cellWithTag:[[backEMFData objectAtIndex:0] intValue]] 
	 setDoubleValue:[[backEMFData objectAtIndex:1] doubleValue]];
	[[backEMFFields cellWithTag:[[backEMFData objectAtIndex:0] intValue]] 
	 setStringValue:[NSString stringWithFormat:@"%0.3lf",[[backEMFData objectAtIndex:1] doubleValue]]];
}

- (void)SensorUpdate:(NSArray *)sensorData
{
	[[sensorSliders cellWithTag:[[sensorData objectAtIndex:0] intValue]] 
	 setDoubleValue:[[sensorData objectAtIndex:1] doubleValue]];
	[[sensorFields cellWithTag:[[sensorData objectAtIndex:0] intValue]] 
	 setIntValue:[[sensorData objectAtIndex:1] intValue]];
}

- (void)EncoderPositionChange:(NSArray *)encoderData
{
	int posn;
	CPhidgetMotorControl_getEncoderPosition(motor, [[encoderData objectAtIndex:0] intValue], &posn);
	[encoderPosition setIntValue:posn];
	[encoderPositionSlider setIntValue:posn];
}

- (void)updateSupplyVoltage
{
	double voltage;
	if(!CPhidgetMotorControl_getSupplyVoltage(motor, &voltage))
	{
		[supplyVoltageField setStringValue:[NSString stringWithFormat:@"%0.2lf", voltage]];
	}
}

/*
* 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 = strtol([[args objectAtIndex:[args count]-1] UTF8String], NULL, 10);
		if(serial == 0) serial = -1;
	}
	
	[mainWindow setDelegate:self];
	
	CPhidget_enableLogging(PHIDGET_LOG_INFO, NULL);
	
	CPhidgetMotorControl_create(&motor);
	
	CPhidgetMotorControl_set_OnInputChange_Handler(motor, gotInputChange, self);
	CPhidgetMotorControl_set_OnVelocityChange_Handler(motor, gotVelocityChange, self);
	CPhidgetMotorControl_set_OnCurrentChange_Handler(motor, gotCurrentChange, self);
	CPhidgetMotorControl_set_OnBackEMFUpdate_Handler(motor, gotBackEMFUpdate, self);
	CPhidgetMotorControl_set_OnSensorUpdate_Handler(motor, gotSensorUpdate, self);
	CPhidgetMotorControl_set_OnEncoderPositionChange_Handler(motor, gotEncoderPositionChange, self);
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)motor, gotAttach, self);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)motor, gotDetach, self);
	
	if(remote)
		CPhidget_openRemote((CPhidgetHandle)motor, serial, NULL, NULL);
	else
		CPhidget_open((CPhidgetHandle)motor, serial);
}

- (void)windowWillClose:(NSNotification *)aNotification {
	//Because this event is called twice
	if(motor)
	{
		CPhidget_close((CPhidgetHandle)motor);
		CPhidget_delete((CPhidgetHandle)motor);
		motor = NULL;
	}
	[NSApp terminate:self];
}

- (void)setPicture:(int)version:(CPhidget_DeviceID)devid
{
	NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
	NSString *imgPath;
	
	switch(devid)
	{
		case PHIDID_MOTORCONTROL_HC_2MOTOR:
			imgPath = [[NSBundle bundleForClass:[self class]] pathForResource:@"1064_0" ofType:@"icns"];
			break;
		case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT:
			imgPath = [[NSBundle bundleForClass:[self class]] pathForResource:@"1060_0" ofType:@"icns"];
			break;
		case PHIDID_MOTORCONTROL_1MOTOR:
			imgPath = [[NSBundle bundleForClass:[self class]] pathForResource:@"1065_0" ofType:@"icns"];
			break;
		default:
			imgPath = nil;
			break;
	}
	
	NSImage *img = [[NSImage alloc]  initByReferencingFile:imgPath];
	
	//otherwise the images are just painted over each other - and the transparency causes trouble
	[pictureBox setImage:nil];
	[pictureBox display];
	if(imgPath!=nil)
		[NSApp setApplicationIconImage: img];
	[pictureBox setImage:img];
	[pictureBox display];
	
	[pool release];
}
@end
