/*  libfreenect - an open source Kinect driver

Copyright (C) 2010  Hector Martin "marcan" <hector@marcansoft.com>

This code is licensed to you under the terms of the GNU GPL, version 2 or version 3;
see:
 http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
 http://www.gnu.org/licenses/gpl-3.0.txt
*/

// Notes (Falcon4ever): Just a simple demo based on the code from github.
// Its marcan's code and with some ported stuff from the C# example

#include <stdio.h>
#include <usb.h>
#include "libfreenect.h"

enum KinectLEDStatus
{
    Off = 0x0,
    Green = 0x1,
    Red = 0x2,
    Yellow = 0x3,
    BlinkingYellow = 0x4,
    BlinkingGreen = 0x5,
    AlternateRedYellow = 0x6,
    AlternateRedGreen = 0x7
};

// Set Led
void SetLED(usb_dev_handle* s, int status)
{	
	int ret = usb_control_msg(s, 0x40, 0x06, status, 0, 0, 0, 10);
}

// Set Tilt
void SetTilt(usb_dev_handle* s, byte tiltValue)
{
	byte mappedValue = (byte)(0xffd0 + tiltValue / 5);
	int ret = usb_control_msg(s, 0x40, 0x31, mappedValue, 0, 0, 0, 10);	
}

struct usb_bus* bus = NULL;
struct usb_device* dev = NULL;
struct usb_dev_handle* s = NULL;

int main(int argc, char **argv)
{	
	

	printf("Kinect camera test\n");

	///////////////////////

	usb_init();
	usb_find_busses();
    usb_find_devices();

	for (bus = usb_get_busses(); bus != 0; bus = bus->next) 
	{			
		for (dev = bus->devices; dev != 0; dev = dev->next) 
		{	
			if (dev->descriptor.idVendor == 0x045E && 
				dev->descriptor.idProduct == 0x02B0)
			{
				s = usb_open(dev);
				if (!s) 
				{
					printf("Can't open device!\n");
					return 1;
				}
				break;
			}
		}
	}	

	if(!s)
	{
		printf("Can't query busses or find device!\n");
		return 1;
	}

	if(usb_set_configuration(s, 1) < 0)
	{
		printf("Can't set!\n");
		return 1;
	}

	if(usb_claim_interface(s, 0) < 0)
	{
		printf("Can't claim!\n");
		return 1;
	}

	// Do smth here
	SetTilt(s, 50);
	SetLED(s, Red);
	Sleep(2000);
	
	SetTilt(s, 255);
	SetLED(s, Yellow);
	Sleep(2000);

	SetTilt(s, 128);
	SetLED(s, BlinkingYellow);
	Sleep(2000);

	// Ok quit the crap, reset
	SetTilt(s, 255);
	SetLED(s, Off);

	// Close USB
	usb_close(s);
		
	return 0;
}