/* original code by Dominic Szablewski released into public domain at
 * http://www.phoboslab.org/log/2009/07/uvc-camera-control-for-mac-os-x
 
   Modified by Simon Urbanek:
   - added UVCDevice class to support enumeration of UVC devices
   - removed hard-coded unit positions
   - added interface detection (so it works with other cameras including C910)
   - added focus controls
 
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:
     * Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
     * Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
     * Neither the name of the author, associated companies nor the
       names of its contributors may be used to endorse or promote products
       derived from this software without specific prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.The 
 */
 
#import "UVCCameraControl.h"

/*--- constants from the UVC standard
 http://www.usb.org/developers/devclass_docs/USB_Video_Class_1_1.zip
 ---*/

/* video interface class */
#define CC_VIDEO           0x0E

/* video interface subclasses */
#define SC_VIDEOCONTROL    0x01
#define SC_VIDEOSTREAMING  0x02

/* interface descriptor types */
#define CS_INTERFACE       0x24

/* interface descriptor subtypes */
#define VC_HEADER          0x01
#define VC_INPUT_TERMINAL  0x02
#define VC_OUTPUT_TERMINAL 0x03
#define VC_SELECTOR_UNIT   0x04
#define VC_PROCESSING_UNIT 0x05
#define VC_EXTENSION_UNIT  0x06


/* interface descriptor subtypes (video control) */
/* NOTE: those are virtual types inherited form the old code! The unit IDs are device-specific so they have to be mapped */
#define UVC_INPUT_TERMINAL_ID  0x01        /* VC_INPUT_TERMINAL   0x02 --- ??? */
#define UVC_PROCESSING_UNIT_ID 0x02        /* VC_PROCESSING_UNIT  0x05 --- USB prober unit id = 2 */

/* interface class */
#define UVC_CONTROL_INTERFACE_CLASS    CC_VIDEO         /* CC_VIDEO         0x0E */

/* interface subclass */
#define UVC_CONTROL_INTERFACE_SUBCLASS SC_VIDEOCONTROL  /* SC_VIDEOCONTROL   0x01 */

#define UVC_SET_CUR	0x01
#define UVC_GET_CUR	0x81
#define UVC_GET_MIN	0x82
#define UVC_GET_MAX	0x83

typedef struct CSInterfaceDescriptor {
	unsigned char bLength, bDescriptorType, bDescriptorSubType, bUnitID;
} CSInterfaceDescriptor;


typedef struct {
	uvc_control_info_t autoExposure;
	uvc_control_info_t absoluteFocus;
	uvc_control_info_t autoFocus;
	uvc_control_info_t exposure;
	uvc_control_info_t brightness;
	uvc_control_info_t contrast;
	uvc_control_info_t gain;
	uvc_control_info_t saturation;
	uvc_control_info_t sharpness;
	uvc_control_info_t whiteBalance;
	uvc_control_info_t autoWhiteBalance;
} uvc_controls_t ;

const uvc_controls_t uvc_controls = {
	.autoExposure = {
		.unit = UVC_INPUT_TERMINAL_ID,
		.selector = 0x02,
		.size = 1,
	},
	.absoluteFocus = {
		.unit = UVC_INPUT_TERMINAL_ID,
		.selector = 0x06, // CT_FOCUS_ABSOLUTE_CONTROL
		.size = 2,
	}, 
	.autoFocus = {
		.unit = UVC_INPUT_TERMINAL_ID,
		.selector = 0x08, // CT_FOCUS_AUTO_CONTROL
		.size = 1,
	},
	.exposure = {
		.unit = UVC_INPUT_TERMINAL_ID,
		.selector = 0x04,
		.size = 4,
	},
	.brightness = {
		.unit = UVC_PROCESSING_UNIT_ID,
		.selector = 0x02,
		.size = 2,
	},
	.contrast = {
		.unit = UVC_PROCESSING_UNIT_ID,
		.selector = 0x03,
		.size = 2,
	},
	.gain = {
		.unit = UVC_PROCESSING_UNIT_ID,
		.selector = 0x04,
		.size = 2,
	},
	.saturation = {
		.unit = UVC_PROCESSING_UNIT_ID,
		.selector = 0x07,
		.size = 2,
	},
	.sharpness = {
		.unit = UVC_PROCESSING_UNIT_ID,
		.selector = 0x08,
		.size = 2,
	},
	.whiteBalance = {
		.unit = UVC_PROCESSING_UNIT_ID,
		.selector = 0x0A,
		.size = 2,
	},
	.autoWhiteBalance = {
		.unit = UVC_PROCESSING_UNIT_ID,
		.selector = 0x0B,
		.size = 1,
	},
};

/* helper function - takes the device interface and queries it for CC_VIDEO:SC_VIDEOCONTROL interface */
static IOUSBInterfaceInterface220** getControlInterfaceWithDeviceInterface(IOUSBDeviceInterface ** deviceInterface) {
	IOUSBInterfaceInterface220 **controlInterface;
	
	io_iterator_t interfaceIterator;
	IOUSBFindInterfaceRequest interfaceRequest;
	interfaceRequest.bInterfaceClass    = UVC_CONTROL_INTERFACE_CLASS;
	interfaceRequest.bInterfaceSubClass = UVC_CONTROL_INTERFACE_SUBCLASS;
	interfaceRequest.bInterfaceProtocol = kIOUSBFindInterfaceDontCare;
	interfaceRequest.bAlternateSetting  = kIOUSBFindInterfaceDontCare;
	
	IOReturn success = (*deviceInterface)->CreateInterfaceIterator( deviceInterface, &interfaceRequest, &interfaceIterator );
	if( success != kIOReturnSuccess )
		return NULL;

	io_service_t usbInterface;
	HRESULT result;
	
	/* pick the first video control interface */
	if ((usbInterface = IOIteratorNext(interfaceIterator))) {
		IOCFPlugInInterface **plugInInterface = NULL;
		
		//Create an intermediate plug-in
		SInt32 score;
		kern_return_t kr = IOCreatePlugInInterfaceForService( usbInterface, kIOUSBInterfaceUserClientTypeID, kIOCFPlugInInterfaceID, &plugInInterface, &score );
		
		//Release the usbInterface object after getting the plug-in
		kr = IOObjectRelease(usbInterface);
		if( (kr != kIOReturnSuccess) || !plugInInterface ) {
			NSLog( @"CameraControl Error: Unable to create a plug-in (%08x)\n", kr );
                        IOObjectRelease(interfaceIterator);
			return NULL;
		}
		
		//Now create the device interface for the interface
		result = (*plugInInterface)->QueryInterface( plugInInterface, CFUUIDGetUUIDBytes(kIOUSBInterfaceInterfaceID), (LPVOID *) &controlInterface );
		
		//No longer need the intermediate plug-in
		(*plugInInterface)->Release(plugInInterface);
		
		if( result || !controlInterface ) {
			NSLog( @"CameraControl Error: Couldn’t create a device interface for the interface (%08x)", (int) result );
                        IOObjectRelease(interfaceIterator);
			return NULL;
		}
		
                IOObjectRelease(interfaceIterator);
		return controlInterface;
	}
	
	return NULL;
}

@implementation UVCCameraControl

/* enumerates units in the interface in order to create a unit map */
- (void) _enumUnits
{
	memset(unit_map, 0, sizeof(unit_map));
	if (interface) {
		IOUSBDescriptorHeader *desc = 0;
		int i = 0;
		
		while ((desc = (*interface)->FindNextAssociatedDescriptor(interface, desc, kUSBAnyDesc))) {
			++i;
			if (desc->bDescriptorType == CS_INTERFACE) {
				CSInterfaceDescriptor *ide = (CSInterfaceDescriptor*) desc;
				if (ide->bDescriptorSubType == VC_INPUT_TERMINAL && !unit_map[UVC_INPUT_TERMINAL_ID])
					unit_map[UVC_INPUT_TERMINAL_ID] = ide->bUnitID;
				else if (ide->bDescriptorSubType == VC_PROCESSING_UNIT && !unit_map[UVC_PROCESSING_UNIT_ID])
					unit_map[UVC_PROCESSING_UNIT_ID] = ide->bUnitID;
				// printf("   type %02x, subtype %02x, length %d\n", ide->bDescriptorType, ide->bDescriptorSubType, ide->bLength);
			}
		}
		
		// printf("unit map: "); for (i = 0; i < 8; i++) printf(" %02x", unit_map[i]); printf("\n");
	}
}

- (int) unitIdForFunction: (int) fn
{
	if (fn < 0 || fn >= 8) return 0;
	// use the map but if it's empty (enumeration failed) fall back to the function index
	return unit_map[fn] ? unit_map[fn] : fn;
}

/* initializes first UVC device that is found */
- (id) init {
	if ((self = [super init])) {
		interface = NULL;
		
		CFMutableDictionaryRef matchingDict = IOServiceMatching(kIOUSBDeviceClassName);
		io_iterator_t serviceIterator;
		IOServiceGetMatchingServices( kIOMasterPortDefault, matchingDict, &serviceIterator );
		
		io_service_t camera;
		while ((camera = IOIteratorNext(serviceIterator))) {
			// Get DeviceInterface
			unsigned char deviceClass, deviceSubClass, deviceProtocol;
			BOOL valid = NO;
			IOUSBDeviceInterface  **deviceInterface = NULL;
			IOCFPlugInInterface   **plugInInterface = NULL;
			SInt32 score;
			kern_return_t kr = IOCreatePlugInInterfaceForService( camera, kIOUSBDeviceUserClientTypeID, kIOCFPlugInInterfaceID, &plugInInterface, &score );
			if( (kIOReturnSuccess != kr) || !plugInInterface ) {
				NSLog( @"CameraControl Error: IOCreatePlugInInterfaceForService returned 0x%08x.", kr );
				continue;
			}
			
			HRESULT res = (*plugInInterface)->QueryInterface(plugInInterface, CFUUIDGetUUIDBytes(kIOUSBDeviceInterfaceID), (LPVOID*) &deviceInterface );
			(*plugInInterface)->Release(plugInInterface);
			if( res || deviceInterface == NULL ) {
				NSLog( @"CameraControl Error: QueryInterface returned %d.\n", (int)res );
				continue;
			}
			
			/* accodring to the standard the class/subclass/protocol for video devices
			   is 0xEF, 0x02, 0x01 for devices with more than VC and 0x00, *, * for
			   those with only VC -- so we check that to avoid too much interface querying */
			(*deviceInterface)->GetDeviceClass(deviceInterface, &deviceClass);
			if (deviceClass == 0xEF) {
				(*deviceInterface)->GetDeviceSubClass(deviceInterface, &deviceSubClass);
				(*deviceInterface)->GetDeviceProtocol(deviceInterface, &deviceProtocol);
				if (deviceSubClass == 2 && deviceProtocol == 1)
					valid = YES;
			} else if (deviceClass == 0)
				valid = YES;

			if (valid) { /* time to check the interfaces */
				interface = getControlInterfaceWithDeviceInterface(deviceInterface);
				if (interface)
					break;
			}
		}
		[self _enumUnits];
		
	}
	return self;
}

- (id) initWithDeviceInterface: (IOUSBDeviceInterface**) deviceInterface {
	if ((self = [super init])) {
		interface = getControlInterfaceWithDeviceInterface(deviceInterface);
		if (!interface) {
			[self release];
			self = nil;
		}
		[self _enumUnits];
	}
	return self;
}
	
- (id)initWithLocationID:(UInt32)locationID {
	CFMutableDictionaryRef matchingDict = IOServiceMatching(kIOUSBDeviceClassName);
	io_iterator_t serviceIterator;
	IOServiceGetMatchingServices( kIOMasterPortDefault, matchingDict, &serviceIterator );
	
	io_service_t camera;
	while ((camera = IOIteratorNext(serviceIterator))) {
		IOUSBDeviceInterface  **deviceInterface = NULL;
		IOCFPlugInInterface   **plugInInterface = NULL;
		SInt32 score;
		kern_return_t kr = IOCreatePlugInInterfaceForService( camera, kIOUSBDeviceUserClientTypeID, kIOCFPlugInInterfaceID, &plugInInterface, &score );
		if( (kIOReturnSuccess != kr) || !plugInInterface )
			continue;
		
		HRESULT res = (*plugInInterface)->QueryInterface(plugInInterface, CFUUIDGetUUIDBytes(kIOUSBDeviceInterfaceID), (LPVOID*) &deviceInterface );
		(*plugInInterface)->Release(plugInInterface);
		if (res || !deviceInterface)
			continue;
		
		UInt32 currentLocationID = 0;
		(*deviceInterface)->GetLocationID(deviceInterface, &currentLocationID);
		
		if( currentLocationID == locationID ) {
			interface = getControlInterfaceWithDeviceInterface(deviceInterface);
			break;
		}
	}
	[self _enumUnits];
	return self;
}


- (id)initWithVendorID:(long)vendorID productID:(long)productID {
	if ((self = [super init])) {
		interface = NULL;
		
		// Find USB Device
		CFMutableDictionaryRef matchingDict = IOServiceMatching(kIOUSBDeviceClassName);
		CFNumberRef numberRef;
		
		numberRef = CFNumberCreate(kCFAllocatorDefault, kCFNumberSInt32Type, &vendorID);
		CFDictionarySetValue( matchingDict, CFSTR(kUSBVendorID), numberRef );
		CFRelease(numberRef);
		
		numberRef = CFNumberCreate(kCFAllocatorDefault, kCFNumberSInt32Type, &productID);
		CFDictionarySetValue( matchingDict, CFSTR(kUSBProductID), numberRef );
		CFRelease(numberRef);
		io_service_t camera = IOServiceGetMatchingService( kIOMasterPortDefault, matchingDict );
		
		
		// Get DeviceInterface
		IOUSBDeviceInterface  **deviceInterface = NULL;
		IOCFPlugInInterface   **plugInInterface = NULL;
		SInt32 score;
		kern_return_t kr = IOCreatePlugInInterfaceForService( camera, kIOUSBDeviceUserClientTypeID, kIOCFPlugInInterfaceID, &plugInInterface, &score );
		if( (kIOReturnSuccess != kr) || !plugInInterface ) {
			NSLog( @"CameraControl Error: IOCreatePlugInInterfaceForService returned 0x%08x.", kr );
			return self;
		}
		
		HRESULT res = (*plugInInterface)->QueryInterface(plugInInterface, CFUUIDGetUUIDBytes(kIOUSBDeviceInterfaceID), (LPVOID*) &deviceInterface );
		(*plugInInterface)->Release(plugInInterface);
		if( res || deviceInterface == NULL ) {
			NSLog( @"CameraControl Error: QueryInterface returned %d.\n", (int)res );
			return self;
		}
		
		interface = getControlInterfaceWithDeviceInterface(deviceInterface);

		[self _enumUnits];
	}
	return self;
}


- (void)dealloc {
	if( interface ) {
		(*interface)->USBInterfaceClose(interface);
		(*interface)->Release(interface);
	}
	[super dealloc];
}


- (BOOL)sendControlRequest:(IOUSBDevRequest)controlRequest {
	unsigned char interfaceNumber;
	if( !interface ){
		NSLog( @"CameraControl Error: No interface to send request" );
		return NO;
	}
	
	//Now open the interface. This will cause the pipes associated with
	//the endpoints in the interface descriptor to be instantiated
	kern_return_t kr = (*interface)->USBInterfaceOpen(interface);
	if (kr != kIOReturnSuccess)	{
		NSLog( @"CameraControl Error: Unable to open interface (%08x)\n", kr );
		return NO;
	}
	// wIndex needs the interface number, so we have to pull it from interface (we could cache it but it's not worth it ...)
	kr = (*interface)->GetInterfaceNumber(interface, &interfaceNumber);
	if (kr == kIOReturnSuccess)
		controlRequest.wIndex |= interfaceNumber;
	kr = (*interface)->ControlRequest(interface, 0, &controlRequest);
	if( kr != kIOReturnSuccess ) {
		NSLog( @"CameraControl Error: Control request failed: %08x", kr );
		kr = (*interface)->USBInterfaceClose(interface);
		return NO;
	}
	
	kr = (*interface)->USBInterfaceClose(interface);
	
	return YES;
}


- (BOOL)setData:(long)value withLength:(int)length forSelector:(int)selector at:(int)unitId {
	IOUSBDevRequest controlRequest;
	controlRequest.bmRequestType = USBmakebmRequestType( kUSBOut, kUSBClass, kUSBInterface );
	controlRequest.bRequest = UVC_SET_CUR;
	controlRequest.wValue = selector << 8;
	controlRequest.wIndex = [self unitIdForFunction:unitId] << 8; // sendControlRequest will fill-in the interface number
	controlRequest.wLength = length;
	controlRequest.wLenDone = 0;
	controlRequest.pData = &value;
	return [self sendControlRequest:controlRequest];
}


- (long)getDataFor:(int)type withLength:(int)length fromSelector:(int)selector at:(int)unitId {
	long value = 0;
	IOUSBDevRequest controlRequest;
	controlRequest.bmRequestType = USBmakebmRequestType( kUSBIn, kUSBClass, kUSBInterface );
	controlRequest.bRequest = type;
	controlRequest.wValue = selector << 8;
	controlRequest.wIndex = [self unitIdForFunction:unitId] << 8;
	controlRequest.wLength = length;
	controlRequest.wLenDone = 0;
	controlRequest.pData = &value;
	BOOL success = [self sendControlRequest:controlRequest];
	return ( success ? value : 0 );
}


// Get Range (min, max)
- (uvc_range_t)getRangeForControl:(const uvc_control_info_t *)control {
	uvc_range_t range = { 0, 0 };
	range.min = [self getDataFor:UVC_GET_MIN withLength:control->size fromSelector:control->selector at:control->unit];
	range.max = [self getDataFor:UVC_GET_MAX withLength:control->size fromSelector:control->selector at:control->unit];
	return range;
}


// Used to de-/normalize values
- (float)mapValue:(float)value fromMin:(float)fromMin max:(float)fromMax toMin:(float)toMin max:(float)toMax {
	return toMin + (toMax - toMin) * ((value - fromMin) / (fromMax - fromMin));
}


// Get a normalized value
- (float)getValueForControl:(const uvc_control_info_t *)control {
	// TODO: Cache the range somewhere?
	uvc_range_t range = [self getRangeForControl:control];
	
	int intval = [self getDataFor:UVC_GET_CUR withLength:control->size fromSelector:control->selector at:control->unit];
	return [self mapValue:intval fromMin:range.min max:range.max toMin:0 max:1];
}


// Set a normalized value
- (BOOL)setValue:(float)value forControl:(const uvc_control_info_t *)control {
	// TODO: Cache the range somewhere?
	uvc_range_t range = [self getRangeForControl:control];
	
	int intval = [self mapValue:value fromMin:0 max:1 toMin:range.min max:range.max];
	return [self setData:intval withLength:control->size forSelector:control->selector at:control->unit];
}





// ================================================================

// Set/Get the actual values for the camera
//

- (BOOL)setAutoExposure:(BOOL)enabled {
	int intval = (enabled ? 0x08 : 0x01); // "auto exposure modes" ar NOT boolean (on|off) as it seems
	return [self setData:intval 
			  withLength:uvc_controls.autoExposure.size 
			 forSelector:uvc_controls.autoExposure.selector 
					  at:uvc_controls.autoExposure.unit];
}

- (BOOL)getAutoExposure {
	int intval = [self getDataFor:UVC_GET_CUR 
					   withLength:uvc_controls.autoExposure.size 
					 fromSelector:uvc_controls.autoExposure.selector 
							   at:uvc_controls.autoExposure.unit];
	
	return ( intval == 0x08 ? YES : NO );
}

- (BOOL)setExposure:(float)value {
	value = 1 - value;
	return [self setValue:value forControl:&uvc_controls.exposure];
}

- (float)getExposure {
	float value = [self getValueForControl:&uvc_controls.exposure];
	return 1 - value;
}

- (BOOL)setGain:(float)value {
	return [self setValue:value forControl:&uvc_controls.gain];
}

- (float)getGain {
	return [self getValueForControl:&uvc_controls.gain];
}

- (BOOL)setBrightness:(float)value {
	return [self setValue:value forControl:&uvc_controls.brightness];
}

- (float)getBrightness {
	return [self getValueForControl:&uvc_controls.brightness];
}

- (BOOL)setContrast:(float)value {
	return [self setValue:value forControl:&uvc_controls.contrast];
}

- (float)getContrast {
	return [self getValueForControl:&uvc_controls.contrast];
}

- (BOOL)setSaturation:(float)value {
	return [self setValue:value forControl:&uvc_controls.saturation];
}

- (float)getSaturation {
	return [self getValueForControl:&uvc_controls.saturation];
}

- (BOOL)setSharpness:(float)value {
	return [self setValue:value forControl:&uvc_controls.sharpness];
}

- (float)getSharpness {
	return [self getValueForControl:&uvc_controls.sharpness];
}

- (BOOL)setFocus:(float)value {
	return [self setValue:value forControl:&uvc_controls.absoluteFocus];
}

- (float)getFocus {
	return [self getValueForControl:&uvc_controls.absoluteFocus];
}

- (BOOL)setAutoWhiteBalance:(BOOL)enabled {
	int intval = (enabled ? 0x01 : 0x00);
	return [self setData:intval 
			  withLength:uvc_controls.autoWhiteBalance.size 
			 forSelector:uvc_controls.autoWhiteBalance.selector 
					  at:uvc_controls.autoWhiteBalance.unit];
}

- (BOOL)getAutoWhiteBalance {
	int intval = [self getDataFor:UVC_GET_CUR 
			   withLength:uvc_controls.autoWhiteBalance.size 
			 fromSelector:uvc_controls.autoWhiteBalance.selector 
				   at:uvc_controls.autoWhiteBalance.unit];
	
	return ( intval ? YES : NO );
}

- (BOOL)getAutoFocus {
	int intval = [self getDataFor:UVC_GET_CUR 
					   withLength:uvc_controls.autoFocus.size 
					 fromSelector:uvc_controls.autoFocus.selector 
							   at:uvc_controls.autoFocus.unit];
	
	return ( intval ? YES : NO );
}

- (BOOL)setAutoFocus:(BOOL)enabled {
	int intval = (enabled ? 0x01 : 0x00);
	return [self setData:intval 
		  withLength:uvc_controls.autoFocus.size 
		 forSelector:uvc_controls.autoFocus.selector 
			  at:uvc_controls.autoFocus.unit];
}

- (BOOL)setWhiteBalance:(float)value {
	return [self setValue:value forControl:&uvc_controls.whiteBalance];
}

- (float)getWhiteBalance {
	return [self getValueForControl:&uvc_controls.whiteBalance];
}


@end