#include <stdio.h>  /* Brauchen wir für Ein-/ bzw. Ausgabe */
#include <string.h> /* Brauchen wir für die Auswertung von Parametern */
#include <usb.h>    /* Die Bibliothek - Liegt diese im Projektverzeichnis so muss #include "usb.h" verwendet werden*/
#include <iostream>

#define USBDEV_SHARED_VENDOR    0x16C0  /* VOTI */
#define USBDEV_SHARED_PRODUCT   0x27d8  /* Obdev's free shared PID */

#define CMD_SETSERVO   0
#define CMD_GETSERVO   1
#define CMD_SETLED     2
#define CMD_GETLED     3

static void usage(char *name)
{
  fprintf(stderr, "usage:\n");
  fprintf(stderr, "  %s on\n", name);
  fprintf(stderr, "  %s off\n", name);
  fprintf(stderr, "  %s servo [0-255] [0-n]\n", name);
  fprintf(stderr, "  %s status [0-n]\n", name);
}

static int usbGetStringAscii(usb_dev_handle *dev, int index, int langid, char *buf, int buflen)
{
  char buffer[256];
  int rval, i;

  if ((rval = usb_control_msg(dev, USB_ENDPOINT_IN, USB_REQ_GET_DESCRIPTOR, (USB_DT_STRING << 8) + index, langid, buffer, sizeof (buffer), 1000)) < 0)
    return rval;
  if (buffer[1] != USB_DT_STRING)
    return 0;
  if ((unsigned char) buffer[0] < rval)
    rval = (unsigned char) buffer[0];
  rval /= 2;
  /* lossy conversion to ISO Latin1 */
  for (i = 1; i < rval; i++) {
    if (i > buflen) /* destination buffer overflow */
      break;
    buf[i - 1] = buffer[2 * i];
    if (buffer[2 * i + 1] != 0) /* outside of ISO Latin1 range */
      buf[i - 1] = '?';
  }
  buf[i - 1] = 0;
  return i - 1;
}

#define USB_ERROR_NOTFOUND  1
#define USB_ERROR_ACCESS    2
#define USB_ERROR_IO        3

static int usbOpenDevice(usb_dev_handle **device, int vendor, char *vendorName, int product, char *productName)
{
  struct usb_bus *bus;
  struct usb_device *dev;
  usb_dev_handle *handle = NULL;
  int errorCode = USB_ERROR_NOTFOUND;
  static int didUsbInit = 0;

  if (!didUsbInit) {
    didUsbInit = 1;
    usb_init();
  }
  usb_find_busses();
  usb_find_devices();
  for (bus = usb_get_busses(); bus; bus = bus->next) {
    for (dev = bus->devices; dev; dev = dev->next) {
      //std::cout << dev->descriptor.idVendor << " " << vendor << " ";
      if (dev->descriptor.idVendor == vendor && dev->descriptor.idProduct == product) {
        char string[256];
        int len;
        handle = usb_open(dev); /* we need to open the device in order to query strings */
        if (!handle) {
          errorCode = USB_ERROR_ACCESS;
          fprintf(stderr, "Warning: cannot open USB device: %s\n", usb_strerror());
          continue;
        }
        if (vendorName == NULL && productName == NULL) { /* name does not matter */
          break;
        }
        /* now check whether the names match: */
        len = usbGetStringAscii(handle, dev->descriptor.iManufacturer, 0x0409, string, sizeof (string));
        if (len < 0) {
          errorCode = USB_ERROR_IO;
          fprintf(stderr, "Warning: cannot query manufacturer for device: %s\n", usb_strerror());
        } else {
          errorCode = USB_ERROR_NOTFOUND;
          /* fprintf(stderr, "seen device from vendor ->%s<-\n", string); */
          if (strcmp(string, vendorName) == 0) {
            len = usbGetStringAscii(handle, dev->descriptor.iProduct, 0x0409, string, sizeof (string));
            if (len < 0) {
              errorCode = USB_ERROR_IO;
              fprintf(stderr, "Warning: cannot query product for device: %s\n", usb_strerror());
            } else {
              errorCode = USB_ERROR_NOTFOUND;
              if (strcmp(string, productName) == 0)
                break;
            }
          }
        }
        usb_close(handle);
        handle = NULL;
      }
    }
    if (handle)
      break;
  }
  if (handle != NULL) {
    errorCode = 0;
    *device = handle;
  }
  return errorCode;
}

int main(int argc, char **argv)
{
  usb_dev_handle *handle = NULL;
  unsigned char buffer[8];
  int nBytes;

  if (argc < 2) {
    usage(argv[0]);
    exit(1);
  }

  usb_init();
  //if (usbOpenDevice(&handle, USBDEV_SHARED_VENDOR, "Weinzierl Stefan", USBDEV_SHARED_PRODUCT, "Board 1") != 0) {
  if (usbOpenDevice(&handle, USBDEV_SHARED_VENDOR, NULL, USBDEV_SHARED_PRODUCT, NULL) != 0) {
    fprintf(stderr, "Could not find USB device \"Board 1\" with vid=0x%x pid=0x%x\n", USBDEV_SHARED_VENDOR, USBDEV_SHARED_PRODUCT);
    exit(1);
  }

  if (strcmp(argv[1], "servo") == 0) {
    if (argc < 4) {
      usage(argv[0]);
      exit(1);
    }
    nBytes = usb_control_msg(handle, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_IN, CMD_SETSERVO, atoi(argv[2]), atoi(argv[3]), (char *) buffer, sizeof (buffer), 5000);
  } else if (strcmp(argv[1], "off") == 0) {
    nBytes = usb_control_msg(handle, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_IN, CMD_SETLED, 0, 255, (char *) buffer, sizeof (buffer), 5000);
  } else if (strcmp(argv[1], "on") == 0) {
    nBytes = usb_control_msg(handle, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_IN, CMD_SETLED, 255, 255, (char *) buffer, sizeof (buffer), 5000);
  } else if (strcmp(argv[1], "status") == 0 && argc==3) {
    nBytes = usb_control_msg(handle, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_IN, CMD_GETSERVO, 0, atoi(argv[2]), (char *) buffer, sizeof (buffer), 5000);

    if (nBytes < 2) {
      if (nBytes < 0)
        fprintf(stderr, "USB error: %s\n", usb_strerror());
      fprintf(stderr, "only %d bytes status received\n", nBytes);
      exit(1);
    }
    printf("%i\n", (buffer[1]<<8) | buffer[0]);

  } else {
    nBytes = 0;
    usage(argv[0]);
    exit(1);
  }
  if (nBytes < 0)
    fprintf(stderr, "USB error: %s\n", usb_strerror());

  usb_close(handle); //USB-Gerät schließen
  return 0;
}