/*
 * Copyright (C) 2007 Adriaan de Groot <groot@kde.org>
 *
 * Written at the Sun stand at LinuxTag while Arne and Sebastian
 * sat across from me doing the same thing. And they won.
 *
Redistribution and use in source and binary forms, with or without  modification, are permitted provided that the following conditions are met:

   1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
   2. 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.
   3. The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 THE AUTHOR 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.
 *
 */
#include <stdio.h>
#include <usb.h>

/*
 * Convenience function returns a (static) pointer to a 
 * buffer containing the string with index i in the device.
 */
static char buf[512];
char *get_string(usb_dev_handle *h, int i)
{
	int l = usb_get_string_simple(h, i, buf, sizeof(buf));
	return (l<0) ? NULL : buf;
}

/*
 * A bunch of debugging cruft while I got the hang of the
 * libusb interfaces; behaves as lsusb.
 */
void describe_device(struct usb_device *p)
{
	usb_dev_handle *d = NULL;
	char *s;
	int l;

	printf("Device @%p name:%s dsize=%d\n", (void *)p, p->filename, p->descriptor.bLength);
	printf("  USBSpec:%d Class:%02x%02x/%02x\n", p->descriptor.bcdUSB,
		p->descriptor.bDeviceClass, 
		p->descriptor.bDeviceSubClass, 
		p->descriptor.bDeviceProtocol);
	printf("  Vendor:%04x (%d) Product:%04x (%d)\n",
		p->descriptor.idVendor, 
		p->descriptor.iManufacturer,
		p->descriptor.idProduct,
		p->descriptor.iProduct
		);

	d = usb_open(p);
	if (NULL == d)
	{
		printf("* Could not open device.\n");
		return;
	}
	
	s = get_string(d,p->descriptor.iManufacturer);
	if (s)
	{
		printf("  Vendor: (%s)\n", s);
	}

	s = get_string(d, p->descriptor.iProduct);
	if (s)
	{
		printf("  Product: (%s)\n", s);
	}


	usb_close(d);
}

void describe_devices(struct usb_device *p)
{
	while(p)
	{
		describe_device(p);
		p = p->next;
	}
}

void describe_bus(struct usb_bus *p)
{
	printf("Bus @%p location:%d name:%s\n", (void *)p, p->location, p->dirname);
	describe_devices(p->devices);
}

void describe_busses(struct usb_bus *p)
{
	while(p)
	{
		describe_bus(p);
		p = p->next;
	}
}

void describe_configuration(usb_dev_handle *d, struct usb_config_descriptor *p)
{
	char *s;
	s = get_string(d,p->iConfiguration);
	printf("  Interfaces: %d config %d (%s)\n", p->bNumInterfaces, 
		p->iConfiguration, s ? s : "<none>");
	s = get_string(d,p->interface->altsetting->iInterface);
	printf("  Interface 0 (%s) endpoints %d\n",
		s ? s : "<none>",
		p->interface->altsetting->bNumEndpoints);
}


/*
 * Search all attached USB devices for a given vendor:product
 * combination and return the USB device pointer for it
 * or NULL if not found.
 */
struct usb_device *find_device(uint16_t vendor, uint16_t product)
{
	struct usb_bus *b = usb_busses;
	struct usb_device *p;
	while (b)
	{
		p = b->devices;
		while(p)
		{
			if ((vendor == p->descriptor.idVendor) &&
				(product == p->descriptor.idProduct)) 
			{
				return p;
			}
			p = p->next;
		}
		b = b->next;	
	}

	return NULL;
}

/*
 * Return an open device handle to the robot if the robot
 * is attached and on or NULL otherwise.
 */
usb_dev_handle *open_robot()
{
	/*  Vendor:0694 (0) Product:0002 (0) 
	 * 	IDs obtained by plugging in the robot and reading
	 *      the lsusb output.
	 */
	struct usb_device *p = find_device(0x0694, 0x0002);
	if (p)
	{
		printf("Robot found, %d configurations\n",
			p->descriptor.bNumConfigurations);
		usb_dev_handle *robot=usb_open(p);
		if (robot)
		{
			describe_configuration(robot,p->config);
			return robot;	
		}
	}

	return NULL;
}


/*
 * Given an open USB device, write a single packet to it
 * which will activate all motors. This causes thrashing.
 */
void communicate(usb_dev_handle *robot)
{
	char *s;
	int interface;
	int r;
	unsigned char cmd[] = { 0, 4, 0xff, 100, 1, 1, 0, 32, 0, 0, 0, 0, 0 };

	/* Just rudely grab interface 0, which is the only one the robot 
	 * has; endpoint 1 is then the motor control endpoint, which you
	 * bulk write to.
	 */
	interface = usb_claim_interface(robot, 0);
	printf("  Got interface %d\n",interface);

	if (interface>=0)
	{
		r = usb_bulk_write(robot, 1, cmd, sizeof(cmd), 50);
		usb_release_interface(robot, interface);
	}
}


/*
 * Open the attached robot device and send a single motor packet.
 */
int main(int argc, char **argv)
{
	int bus_count = 0, device_count = 0;
	usb_dev_handle *d = NULL;

	usb_init();
	bus_count = usb_find_busses();
	printf("USB Bus count=%d\n", bus_count);
	device_count = usb_find_devices();
	printf("USB Device count=%d\n", device_count);

	describe_busses(usb_busses);

	d = open_robot();
	if (NULL == d)
	{
		printf("* No robot found.\n");
		return 1;
	}
	communicate(d);

	usb_close(d);

	return 0;
}