// linmctool - Command-line tool for motion-sensing Bluetooth controllers.
// Copyright (c) 2010,2011 pabr@pabr.org
// See http://www.pabr.org/linmctool/
//
// Compile with: gcc --std=gnu99 -Wall linmctool.c -lusb -o linmctool

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/l2cap.h>

// Global configuration

int verbose = 0;
int enable_ir = 0;
int enable_wmp = 0;
int def_rgb[3] = { 0, 0, 0 };
int output_text = 0;
int output_binary = 0;
int dump_readable = 0;
int dump_writable = 0;
int repeat_dump = 1;
int poll_report = -1;
char *usb_force_master = NULL;
int force_sixaxis = 0;
int force_ds3 = 0;
int timestamp = 0;
int nostdin = 0;

void fatal(char *msg) {
  if ( errno ) perror(msg); else fprintf(stderr, "%s\n", msg);
  exit(1);
}

// ----------------------------------------------------------------------
// Replacement for libbluetooth

int mystr2ba(const char *s, bdaddr_t *ba) {
  if ( strlen(s) != 17 ) return 1;
  for ( int i=0; i<6; ++i ) {
    int d = strtol(s+15-3*i, NULL, 16);
    if ( d<0 || d>255 ) return 1;
    ba->b[i] = d;
  }
  return 0;
}

const char *myba2str(const bdaddr_t *ba) {
  static char buf[2][18];  // Static buffer valid for two invocations.
  static int index = 0;
  index = (index+1)%2;
  sprintf(buf[index], "%02x:%02x:%02x:%02x:%02x:%02x",
	  ba->b[5], ba->b[4], ba->b[3], ba->b[2], ba->b[1], ba->b[0]);
  return buf[index];
}

static const char *rtypename[] = { "OTHER", "INPUT", "OUTPUT", "FEATURE" };

// ----------------------------------------------------------------------
// USB support

#ifndef WITHOUT_USB

#include <usb.h>
#define USB_DIR_IN 0x80
#define USB_DIR_OUT 0
#define USB_GET_REPORT 0x01
#define USB_SET_REPORT 0x09
#define VENDOR_SONY 0x054c
#define PRODUCT_SIXAXIS_DS3 0x0268
#define PRODUCT_PSMOVE 0x03d5

void usb_dump_readable_reports(usb_dev_handle *devh, int itfnum) {
  for ( int rtype=0; rtype<=3; ++rtype )
    for ( int rid=0; rid<256; ++rid ) {
      unsigned char prev[64];
      for ( int c=0; c<repeat_dump; ++c ) {
	unsigned char r[64];
	int nr = usb_control_msg
	  (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
	   USB_GET_REPORT, (rtype<<8)|rid, itfnum, (void*)r, sizeof(r), 5000);
	if ( c==0 || nr<0 || memcmp(r,prev,nr) ) {
	  printf("  USB READ %-7s 0x%02x %c ",
		 rtypename[rtype], rid, c?'#':'=');
	  if ( nr < 0 )
	    printf("ERR %d", nr);
	  else {
	    printf("[%3d]", nr);
	    for ( int i=0; i<nr; ++i ) printf(" %02x", r[i]);
	  }
	  printf("\n");  fflush(stdout);
	}
	memcpy(prev, r, sizeof(prev));
      }
    }
  exit(0);
}

void usb_dump_writable_reports(usb_dev_handle *devh, int itfnum) {
  for ( int rtype=0; rtype<=3; ++rtype )
    for ( int rid=0; rid<256; ++rid ) {
      unsigned char r[16];
      memset(r, 0, sizeof(r));
      int nr = usb_control_msg
	(devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
	 USB_SET_REPORT, (rtype<<8)|rid, itfnum, (void*)r, sizeof(r), 5000);
      printf("  USB WRITE %-7s 0x%02x [%d] -> [%d] %s\n",
	      rtypename[rtype], rid, sizeof(r), nr, (nr<0)?strerror(-nr):"");
      fflush(stdout);
    }
  exit(0);
}

void usb_poll_report(usb_dev_handle *devh, int itfnum, int report) {
  int rtype = report >> 8;
  int rid = report & 255;
  while ( 1 ) {
    unsigned char r[64];
    int nr = usb_control_msg
      (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
       USB_GET_REPORT, (rtype<<8)|rid, itfnum, (void*)r, sizeof(r), 5000);
    struct timeval tv;
    gettimeofday(&tv, NULL);
    printf("  %10lu.%06ld USB READ %-7s 0x%02x ",
	   tv.tv_sec, tv.tv_usec, rtypename[rtype], rid);
    if ( nr < 0 )
      printf("ERR %d", nr);
    else {
      printf("[%3d]", nr);
      for ( int i=0; i<nr; ++i ) printf(" %02x", r[i]);
    }
    printf("\n");
  }
}

void usb_pair_device(struct usb_device *dev, int itfnum) {

  usb_dev_handle *devh = usb_open(dev);
  if ( ! devh ) fatal("usb_open");
  usb_detach_kernel_driver_np(devh, itfnum);
  int res = usb_claim_interface(devh, itfnum);
  if ( res < 0 ) fatal("usb_claim_interface");

  if ( dump_readable ) usb_dump_readable_reports(devh, itfnum);
  if ( dump_writable ) usb_dump_writable_reports(devh, itfnum);
  if ( poll_report >= 0 ) usb_poll_report(devh, itfnum, poll_report);

  bdaddr_t current_ba;  // Current pairing address.

  switch ( dev->descriptor.idProduct ) {
  case PRODUCT_SIXAXIS_DS3: {
    fprintf(stderr, "USB: SIXAXIS/DS3\n");
    unsigned char msg[8];
    res = usb_control_msg
      (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
       USB_GET_REPORT, 0x03f5, itfnum, (void*)msg, sizeof(msg), 5000);
    if ( res < 0 ) fatal("usb_control_msg(read master)");
    for ( int i=0; i<6; ++i ) current_ba.b[i] = msg[7-i];
    break;
  }
  case PRODUCT_PSMOVE: {
    fprintf(stderr, "USB: PS MOVE\n");
    unsigned char msg[16];
    int res = usb_control_msg
      (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
       USB_GET_REPORT, 0x0304, itfnum, (void*)msg, sizeof(msg), 5000);
    if ( res < 0 ) fatal("usb_control_msg(read master)");
    for ( int i=0; i<6; ++i ) current_ba.b[i] = msg[10+i];
    break;
  }
  }

  bdaddr_t ba;  // New pairing address.

  if ( usb_force_master && !mystr2ba(usb_force_master,&ba) )
    ;
  else {
    char ba_s[18];
    FILE *f = popen("hcitool dev", "r");
    if ( !f || fscanf(f, "%*s\n%*s %17s", ba_s)!=1 || mystr2ba(ba_s, &ba) )
      fatal("Unable to retrieve local bd_addr from `hcitool dev`.\n");
    pclose(f);
  }

  // Perform pairing.

  if ( ! bacmp(&current_ba, &ba) ) {
    fprintf(stderr, "  Already paired to %s\n", myba2str(&ba));
  } else {
    fprintf(stderr, "  Changing master from %s to %s\n",
	    myba2str(&current_ba), myba2str(&ba));
    switch ( dev->descriptor.idProduct ) {
    case PRODUCT_SIXAXIS_DS3: {
      char msg[8] =
	{ 0x01, 0x00, ba.b[5],ba.b[4],ba.b[3],ba.b[2],ba.b[1],ba.b[0] };
      res = usb_control_msg
	(devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
	 USB_SET_REPORT, 0x03f5, itfnum, msg, sizeof(msg), 5000);
      if ( res < 0 ) fatal("usb_control_msg(write master)");
      break;
    }
    case PRODUCT_PSMOVE: {
      char msg[]= { 0x05, ba.b[0], ba.b[1], ba.b[2], ba.b[3],
		    ba.b[4], ba.b[5], 0x10, 0x01, 0x02, 0x12 };
      res = usb_control_msg
	(devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
	 USB_SET_REPORT, 0x0305, itfnum, msg, sizeof(msg), 5000);
      if ( res < 0 ) fatal("usb_control_msg(write master)");
      break;
    }
    }
  }

  if ( dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3 )
    fprintf(stderr, "  Now unplug the USB cable and press the PS button.\n");
  else
    fprintf(stderr, "  Now press the PS button.\n");
}

void usb_scan() {
  usb_init();
  if ( usb_find_busses() < 0 ) fatal("usb_find_busses");
  if ( usb_find_devices() < 0 ) fatal("usb_find_devices");
  struct usb_bus *busses = usb_get_busses();
  if ( ! busses ) fatal("usb_get_busses");
  
  struct usb_bus *bus;
  for ( bus=busses; bus; bus=bus->next ) {
    struct usb_device *dev;
    for ( dev=bus->devices; dev; dev=dev->next) {
      struct usb_config_descriptor *cfg;
      for ( cfg = dev->config;
	    cfg < dev->config + dev->descriptor.bNumConfigurations;
	    ++cfg ) {
	int itfnum;
	for ( itfnum=0; itfnum<cfg->bNumInterfaces; ++itfnum ) {
	  struct usb_interface *itf = &cfg->interface[itfnum];
	  struct usb_interface_descriptor *alt;
	  for ( alt = itf->altsetting;
		alt < itf->altsetting + itf->num_altsetting;
		++alt ) {
	    if ( dev->descriptor.idVendor == VENDOR_SONY &&
		 (dev->descriptor.idProduct == PRODUCT_SIXAXIS_DS3 ||
		  dev->descriptor.idProduct == PRODUCT_PSMOVE) &&
		 alt->bInterfaceClass == 3 )
	      usb_pair_device(dev, itfnum);
	  }
	}
      }
    }
  }
}

#else // WITHOUT_USB

void usb_scan() { }

#endif

/**********************************************************************/
// Bluetooth HID devices

static const char *devtypename[] =
  { "Wiimote", "Sixaxis", "DS3", "PS Move" };

struct motion_dev {
  int index;
  bdaddr_t addr;
  enum { WIIMOTE, SIXAXIS, DS3, PSMOVE } type;
  int csk; 
  int isk;
  int rgb[3];  // For PSMOVE
  time_t latest_refresh;  // For PSMOVE. 0 if inactive.
  struct motion_dev *next;
};

#define L2CAP_PSM_HIDP_CTRL 0x11
#define L2CAP_PSM_HIDP_INTR 0x13

#define HIDP_TRANS_GET_REPORT    0x40
#define HIDP_TRANS_SET_REPORT    0x50
#define HIDP_DATA_RTYPE_INPUT    0x01
#define HIDP_DATA_RTYPE_OUTPUT   0x02
#define HIDP_DATA_RTYPE_FEATURE  0x03

void bluetooth_dump_readable_reports(int csk) {
  for ( int rtype=0; rtype<=3; ++rtype )
    for ( int rid=0; rid<256; ++rid ) {
      unsigned char prev[64];
      for ( int c=0; c<repeat_dump; ++c ) {
	unsigned char r[66];
	char get[] = { HIDP_TRANS_GET_REPORT | rtype | 8,
		       rid, sizeof(r), sizeof(r)>>8 };
	send(csk, get, sizeof(get), 0);
	int nr = recv(csk, r, sizeof(r), 0);
	if ( c==0 || nr<0 || memcmp(r,prev,nr) ) {
	  printf("  BLUETOOTH READ %-7s 0x%02x %c ",
		 rtypename[rtype], rid, c?'#':'=');
	  if ( nr < 1 ) printf("ERR %d", nr);
	  else if ( r[0] == (0xa0|rtype) )
	    printf("[%3d]", nr-1);
	  else
	    printf("ERR %d", r[0]);
	  for ( int i=1; i<nr; ++i ) printf(" %02x", r[i]);
	  printf("\n"); fflush(stdout);
	}
	memcpy(prev, r, sizeof(prev));
      }
    }
  exit(0);
}

void bluetooth_dump_writable_reports(int csk) {
  for ( int rtype=0; rtype<=3; ++rtype )
    for ( int rid=0; rid<256; ++rid ) {
      char set[] = { HIDP_TRANS_SET_REPORT | rtype, rid,
		     0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0 };
      send(csk, set, sizeof(set), 0);
      unsigned char ack[16];
      int nr = recv(csk, ack, sizeof(ack), 0);
      printf("  BLUETOOTH WRITE %-7s 0x%02x [%d] -> [%d] %02x\n",
	      rtypename[rtype], rid, sizeof(set)-2, nr, ack[0]);
      fflush(stdout);
    }
  exit(0);
}

void bluetooth_poll_report(int csk, int report) {
  int rtype = report >> 8;
  int rid = report & 255;
  while ( 1 ) {
    unsigned char r[66];
    char get[] = { HIDP_TRANS_GET_REPORT | rtype | 8,
		   rid, sizeof(r), sizeof(r)>>8 };
    send(csk, get, sizeof(get), 0);
    int nr = recv(csk, r, sizeof(r), 0);
    if ( nr < 0 ) fatal("recv");
    struct timeval tv;
    gettimeofday(&tv, NULL);
    printf("  %10lu.%06ld BLUETOOTH READ %-7s 0x%02x ",
	   tv.tv_sec, tv.tv_usec, rtypename[rtype], rid);
    if ( r[0] == (0xa0|rtype) )
      printf("[%3d]", nr-1);
    else
      printf("ERR %d", r[0]);
    for ( int i=1; i<nr; ++i ) printf(" %02x", r[i]);
    printf("\n");
  }
}

// Incoming connections.

int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm) {
  int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
  if ( sk < 0 ) fatal("socket");

  struct sockaddr_l2 addr;
  memset(&addr, 0, sizeof(addr));
  addr.l2_family = AF_BLUETOOTH;
  addr.l2_bdaddr = *BDADDR_ANY;
  addr.l2_psm = htobs(psm);
  if ( bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0 ) {
    perror("bind");
    close(sk);
    return -1;
  }

  if ( listen(sk, 5) < 0 ) fatal("listen");
  return sk;
}

struct motion_dev *accept_device(int csk, int isk) {
  fprintf(stderr, "Incoming connection...\n");
  struct motion_dev *dev = malloc(sizeof(struct motion_dev));
  if ( ! dev ) fatal("malloc");

  dev->csk = accept(csk, NULL, NULL);
  if ( dev->csk < 0 ) fatal("accept(CTRL)");
  dev->isk = accept(isk, NULL, NULL);
  if ( dev->isk < 0 ) fatal("accept(INTR)");

  struct sockaddr_l2 addr;
  socklen_t addrlen = sizeof(addr);
  if ( getpeername(dev->isk, (struct sockaddr *)&addr, &addrlen) < 0 )
    fatal("getpeername");
  dev->addr = addr.l2_bdaddr;
  
  {
    // Distinguish SIXAXIS / DS3 / PSMOVE.
    unsigned char resp[64];
    char get03f2[] = { HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
		       0xf2, sizeof(resp), sizeof(resp)>>8 };
    send(dev->csk, get03f2, sizeof(get03f2), 0);  // 0301 is interesting too.
    int nr = recv(dev->csk, resp, sizeof(resp), 0);
    if ( nr < 19 ) dev->type = PSMOVE;
    else if ( force_sixaxis ) dev->type = SIXAXIS;
    else if ( force_ds3 ) dev->type = DS3;
    else dev->type = (resp[13]==0x40) ? SIXAXIS : DS3;  // My guess.
  }

  return dev;
}

// Outgoing connections.

int l2cap_connect(bdaddr_t *ba, unsigned short psm) {
  int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
  if ( sk < 0 ) fatal("socket");

  struct sockaddr_l2 daddr;
  memset(&daddr, 0, sizeof(daddr));
  daddr.l2_family = AF_BLUETOOTH;
  daddr.l2_bdaddr = *ba;
  daddr.l2_psm = htobs(psm);
  if ( connect(sk, (struct sockaddr *)&daddr, sizeof(daddr)) < 0 )
    fatal("connect");

  return sk;
}

struct motion_dev *connect_device(bdaddr_t *ba) {
  fprintf(stderr, "Connecting to %s\n", myba2str(ba));
  struct motion_dev *dev = malloc(sizeof(struct motion_dev));
  if ( ! dev ) fatal("malloc");
  dev->addr = *ba;
  dev->csk = l2cap_connect(ba, L2CAP_PSM_HIDP_CTRL);
  dev->isk = l2cap_connect(ba, L2CAP_PSM_HIDP_INTR);
  dev->type = WIIMOTE;
  return dev;
}

/**********************************************************************/
// Device setup

#define IR0 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x41
#define IR1 0x40, 0x00

#define BIT1 2

void dump(const char *tag, const unsigned char *buf, int len) {
  fprintf(stderr, "%s[%d]", tag, len);
  for ( int i=0; i<len; ++i ) fprintf(stderr, " %02x", buf[i]);
  fprintf(stderr, "\n");
}

void hidp_trans(int csk, const char *buf, int len) {
  if ( verbose ) dump("SEND", (unsigned char*)buf, len);
  if ( send(csk, buf, len, 0) != len ) fatal("send(CTRL)");
  unsigned char ack;
  int nr = recv(csk, &ack, sizeof(ack), 0);
  if ( verbose) fprintf(stderr, "    result %d  %02x\n", nr, ack);
  if ( nr!=1 || ack!=0 ) fatal("ack");
}
	
void wiimote_command(struct motion_dev *dev, char *req, int n) {
  // Try 3 times.
  for ( int try=0; try<3; ++try ) {
    // Send command
    hidp_trans(dev->csk, req, n);
    // Wait for application-level ack
    for ( int c=0; c<100; ++c ) {
      fd_set fds; FD_ZERO(&fds); FD_SET(dev->isk, &fds);
      struct timeval tv = { .tv_sec=1, .tv_usec=0 };
      if ( select(dev->isk+1,&fds,NULL,NULL,&tv) != 1 ) break;
      unsigned char rsp[16];
      int nr = recv(dev->isk, rsp, sizeof(rsp), 0);
      if ( verbose ) dump("WAIT", rsp, nr);
      if ( nr>=2 && rsp[1]==0x22 ) return;
    }
    fprintf(stderr, "req %02x not acked, retrying\n", req[1]);
  }
  fprintf(stderr, "req %02x failed\n", req[1]);
  fatal("wiimote_command");
}

void setup_device(struct motion_dev *dev) {
  if ( dump_readable ) bluetooth_dump_readable_reports(dev->csk);
  if ( dump_writable ) bluetooth_dump_writable_reports(dev->csk);
  if ( poll_report >= 0 ) bluetooth_poll_report(dev->csk, poll_report);

  switch ( dev->type ) {
  case SIXAXIS:
  case DS3: {
    // Enable reporting
    char set03f4[] = { HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE, 0xf4,
		       0x42, 0x03, 0x00, 0x00 };
    hidp_trans(dev->csk, set03f4, sizeof(set03f4));
    // Leds: Display 1+index in additive format.
    static const char ledmask[10] = { 1, 2, 4, 8, 6, 7, 11, 13, 14, 15 };
    #define LED_PERMANENT 0xff, 0x27, 0x00, 0x00, 0x32
    char set0201[] = {
      HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT, 0x01,
      0x00, 0x00, 0x00, 0,0, 0x00, 0x00, 0x00,
      0x00, ledmask[dev->index%10]<<1,
      LED_PERMANENT,
      LED_PERMANENT,
      LED_PERMANENT,
      LED_PERMANENT,
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    if ( dev->type == SIXAXIS ) {
      set0201[5] = 0xff;  // Enable gyro
      set0201[6] = 0x78;  // Constant bias (should adjust periodically ?)
    } else {
      set0201[5] = 0;     // Set to e.g. 20 to test rumble on startup
      set0201[6] = 0x70;  // Weak rumble
    }
    hidp_trans(dev->csk, set0201, sizeof(set0201));
    break;
  }
  case PSMOVE: {
    memcpy(dev->rgb, def_rgb, sizeof(dev->rgb));
    if ( dev->rgb[0] || dev->rgb[1] || dev->rgb[2] )
      dev->latest_refresh = 1;  // Force refresh in main loop.
    else
      dev->latest_refresh = 0;
    break;
  }
  case WIIMOTE: {
    char req11[] = { 0x52, 0x11, 0x10|BIT1 };  // LED
    wiimote_command(dev, req11, sizeof(req11));
    unsigned char format = 0x31;
    if ( enable_wmp ) {
      char setf0[23] = { 0x52, 0x16, 0x04, 0xa6,0x00,0xf0, 1, 0x55 };
      char setfe[23] = { 0x52, 0x16, 0x04, 0xa6,0x00,0xfe, 1, 0x04 };
      wiimote_command(dev, setf0, sizeof(setf0));
      wiimote_command(dev, setfe, sizeof(setfe));
      format |= 0x04;
    }
    if ( enable_ir ) {
      char req13[] = { 0x52, 0x13, 0x04|BIT1 };  // IR enable 1
      char req1a[] = { 0x52, 0x1a, 0x04|BIT1 };  // IR enable 2
      wiimote_command(dev, req13, sizeof(req13));
      wiimote_command(dev, req1a, sizeof(req1a));

      char set30[23] = { 0x52, 0x16, 0x04, 0xb0,0x00,0x30, 1, 0x01 };
      char set00[23] = { 0x52, 0x16, 0x04, 0xb0,0x00,0x00, 9, IR0 };
      char set1a[23] = { 0x52, 0x16, 0x04, 0xb0,0x00,0x1a, 2, IR1 };  
      int mode = enable_wmp ? 0x01 : 0x03;
      char set33[23] = { 0x52, 0x16, 0x04, 0xb0,0x00,0x33, 1, mode };
      char set30b[23] = { 0x52, 0x16, 0x04, 0xb0,0x00,0x30, 1, 0x08 };
      wiimote_command(dev, set30, sizeof(set30));
      wiimote_command(dev, set00, sizeof(set00));
      wiimote_command(dev, set1a, sizeof(set1a));
      wiimote_command(dev, set33, sizeof(set33));
      wiimote_command(dev, set30b, sizeof(set30b));
      format |= 0x02;
    }
    sleep(1);  // For some reason, this helps.
    char req12[] = { 0x52, 0x12, 0x00|BIT1, format };  // Format 0x33
    wiimote_command(dev, req12, sizeof(req12));
    break;
  }
  }
  fprintf(stderr, "New device %d %s is a %s\n",
	  dev->index, myba2str(&dev->addr), devtypename[dev->type]);
}

void psmove_set_rgb(struct motion_dev *dev, int rgb[3]) {
    char report[] = {
      HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT,
      2, 0, rgb[0],rgb[1],rgb[2], 0, 0
    };
    hidp_trans(dev->csk, report, sizeof(report));
}

/**********************************************************************/
// Reports

void wiimote_parse_accel(unsigned char *r) {
  int aX = r[0] - 128;
  int aY = r[1] - 128;
  int aZ = r[2] - 128;
  printf(" aX=%-4d aY=%-4d aZ=%-4d", aX,aY,aZ);
}

void wiimote_parse_ir10(unsigned char *r) {
  for ( int i=0; i<2; ++i ) {
    unsigned char *p = &r[i*5];
    int x1 = p[0] | ((p[2]<<4)&0x300);
    int y1 = p[1] | ((p[2]<<2)&0x300);
    int x2 = p[3] | ((p[2]<<8)&0x300);
    int y2 = p[4] | ((p[2]<<6)&0x300);
    printf(" ir%dx=%-4d ir%dy=%-4d ir%dx=%-4d ir%dy=%-4d",
	   i*2,x1,i*2,y1, i*2+1,x2,i*2+1,y2);
  }
}

void wiimote_parse_ir12(unsigned char *r) {
  for ( int i=0; i<4; ++i ) {
    unsigned char *p = &r[i*3];
    int x = p[0] | ((p[2]<<4)&0x300);
    int y = p[1] | ((p[2]<<2)&0x300);
    int s = p[2] & 0x0f;
    printf(" ir%dx=%-4d ir%dy=%-4d ir%ds=%-2d", i,x, i,y, i,s);
  }
}

void wiimote_parse_wmp(unsigned char *r) {
  short sgZ = ( ((r[3]&0xfc)<<8) | (r[0]<<2) ) - 32768;
  short sgY = ( ((r[4]&0xfc)<<8) | (r[1]<<2) ) - 32768;
  short sgX = ( ((r[5]&0xfc)<<8) | (r[2]<<2) ) - 32768;
  int gZ = (r[3]&2) ? sgZ>>2 : (sgZ*2000/440)>>2;
  int gX = (r[3]&1) ? sgX>>2 : (sgX*2000/440)>>2;
  int gY = (r[4]&2) ? sgY>>2 : (sgY*2000/440)>>2;
  printf(" gX=%-6d gY=%-6d gZ=%-6d", -gX,gY,gZ);
}

void wiimote_parse_report(unsigned char *r, int len) {
  switch ( r[0] ) {
  case 0x20:
  case 0x22:
    break;
  case 0x31:
    wiimote_parse_accel(r+3);
    break;
  case 0x33:
    wiimote_parse_accel(r+3);
    wiimote_parse_ir12(r+6);
    break;
  case 0x35:
    wiimote_parse_accel(r+3);
    wiimote_parse_wmp(r+6);
    break;
  case 0x37:
    wiimote_parse_accel(r+3);
    wiimote_parse_ir10(r+6);
    wiimote_parse_wmp(r+16);
    break;
  default:
    dump("????", r, len);
    fatal("Unsupported report format");
  }
  printf("\n");
}

void sixaxis_ds3_parse_report(unsigned char *r, int len) {
  if ( r[0]==0x01 && len>=49 ) {
    int aX = r[41]*256 + r[42] - 512;
    int aY = r[43]*256 + r[44] - 512;
    int aZ = r[45]*256 + r[46] - 512;
    int gZ = r[47]*256 + r[48] - 512;
    printf(" aX=%-4d aY=%-4d aZ=%-4d gZ=%-4d", aX,aY,aZ, gZ);
  }
  printf("\n");
}

void psmove_parse_report(unsigned char *r, int len, int latest) {
  if ( r[0]==0x01 && len>=49 ) {
    printf(" seq=%-2d", (r[4]&15)*2+(latest?1:0));
    int ai = latest ? 19 : 13;
    short aX = r[ai+0] + r[ai+1]*256 - 32768;
    short aY = r[ai+2] + r[ai+3]*256 - 32768;
    short aZ = r[ai+4] + r[ai+5]*256 - 32768;
    printf(" aX=%-6d aY=%-6d aZ=%-6d", aX,aY,aZ);
    int ri = latest ? 31 : 25;
    short gX = r[ri+0] + r[ri+1]*256 - 32768;
    short gY = r[ri+2] + r[ri+3]*256 - 32768;
    short gZ = r[ri+4] + r[ri+5]*256 - 32768;
    printf(" gX=%-6d gY=%-6d gZ=%-6d", gX,gY,gZ);
    short mX = (r[38]<<12) | (r[39]<<4);
    short mY = (r[40]<<8)  | (r[41]&0xf0);
    short mZ = (r[41]<<12) | (r[42]<<4);
    mY = - mY;  // Inconsistent sign conventions between acc+gyro and compass.
    printf(" mX=%-5d mY=%-5d mZ=%-5d", mX>>4,mY>>4,mZ>>4);
  }
  printf("\n");
}

void parse_report(struct motion_dev *dev, unsigned char *r, int len) {
  struct timeval tv;
  if ( timestamp ) {
    gettimeofday(&tv, NULL);
    printf("%10lu.%06ld ", tv.tv_sec, tv.tv_usec);
  }
  switch ( dev->type ) {
  case WIIMOTE:
    printf("%d %s WIIMOTE", dev->index, myba2str(&dev->addr));
    wiimote_parse_report(r, len);
    break;
  case SIXAXIS:
    printf("%d %s SIXAXIS", dev->index, myba2str(&dev->addr));
    sixaxis_ds3_parse_report(r, len);
    break;
  case DS3:
    printf("%d %s DS3    ", dev->index, myba2str(&dev->addr));
    sixaxis_ds3_parse_report(r, len);
    break;
  case PSMOVE:  // Two samples per report on accel and gyro axes
    printf("%d %s PSMOVE ", dev->index, myba2str(&dev->addr));
    psmove_parse_report(r, len, 0);
    if ( timestamp ) printf("%10lu.%06ld ", tv.tv_sec, tv.tv_usec);
    printf("%d %s PSMOVE ", dev->index, myba2str(&dev->addr));
    psmove_parse_report(r, len, 1);
    break;
  }
  fflush(stdout);
}

/**********************************************************************/
// Main

void usage() {
  fprintf(stderr, "linmctool version TBD.\n");
  fprintf(stderr, "Usage: linmctool [options] [BDADDR...]\n"
	  "  [--master BDADDR]   Pair USB devices with this adapter\n"
	  "  [--ir]              [WIIMOTE] Enable IR tracking\n"
	  "  [--wmp]             [WIIMOTE] Enable MotionPlus\n"
	  "  [--rgb R,G,B]       [PSMOVE] Default bulb color\n"
	  "  [--timestamp]       Print timestamp\n"
	  "  [--force-sixaxis]   Force detection of SIXAXIS\n"
	  "  [--force-ds3]       Force detection of DS3\n"
	  "  [--binary]          Write raw reports to stdout\n"
	  "  [--verbose]         Print debugging information\n"
	  "  [--dump-readable]   Try to read all reports\n"
	  "  [--dump-writable]   Try to write all reports (dangerous)\n"
	  "  [--repeat-dump N]   Try to write all reports (dangerous)\n"
	  "  [--poll-report N]   Poll (ReportType<<8 | ReportID)\n"
	  "  [--nostdin]         Do not read from standard input\n");
  exit(1);
}

int main(int argc, char *argv[]) {
  struct motion_dev *devs = NULL;
  int next_devindex = 0;

  int csk = l2cap_listen(BDADDR_ANY, L2CAP_PSM_HIDP_CTRL);
  int isk = l2cap_listen(BDADDR_ANY, L2CAP_PSM_HIDP_INTR);
  if ( csk>=0 && isk>=0 )
    fprintf(stderr, "Waiting for Bluetooth connections.\n");
  else
    fprintf(stderr, "Unable to listen on HID PSMs."
	    " Only Wiimote will be supported\n");

  for ( int i=1; i<argc; ++i )
    if ( ! strcmp(argv[i], "--master") && i+1<argc )
      usb_force_master = argv[++i];
    else if ( ! strcmp(argv[i], "--ir") )            enable_ir = 1;
    else if ( ! strcmp(argv[i], "--wmp") )           enable_wmp = 1;
    else if ( ! strcmp(argv[i], "--rgb") && i+1<argc &&
	      sscanf(argv[i+1],"%d,%d,%d",
		     &def_rgb[0],&def_rgb[1],&def_rgb[2])==3 ) ++i;
    else if ( ! strcmp(argv[i], "--timestamp") )     timestamp = 1;
    else if ( ! strcmp(argv[i], "--force-sixaxis") ) force_sixaxis = 1;
    else if ( ! strcmp(argv[i], "--force-ds3") )     force_ds3 = 1;
    else if ( ! strcmp(argv[i], "--text") )          output_text = 1;
    else if ( ! strcmp(argv[i], "--binary") )        output_binary = 1;
    else if ( ! strcmp(argv[i], "--verbose") )       verbose = 1;
    else if ( ! strcmp(argv[i], "--dump-readable") ) dump_readable = 1;
    else if ( ! strcmp(argv[i], "--dump-writable") ) dump_writable = 1;
    else if ( ! strcmp(argv[i], "--repeat-dump") && i+1<argc )
      repeat_dump = atoi(argv[++i]);
    else if ( ! strcmp(argv[i], "--poll-report") && i+1<argc )
      poll_report = strtol(argv[++i], NULL, 0);
    else if ( ! strcmp(argv[i], "--nostdin") ) nostdin = 1;
    else {
      bdaddr_t ba;
      if ( mystr2ba(argv[i], &ba) ) usage();
      struct motion_dev *dev = connect_device(&ba);
      dev->index = next_devindex++;
      dev->next = devs;
      devs = dev;
      setup_device(dev);
    }

  usb_scan();

  if ( ! output_binary ) output_text = 1;

  int binary_skip = 5;  // Avoid confusing binhistogram.

  while ( 1 ) {
    fd_set fds; FD_ZERO(&fds);
    if ( ! nostdin ) FD_SET(0, &fds);
    int fdmax = 0;
    if ( csk >= 0 ) FD_SET(csk, &fds);
    if ( isk >= 0 ) FD_SET(isk, &fds);
    if ( csk > fdmax ) fdmax = csk;
    if ( isk > fdmax ) fdmax = isk;
    for ( struct motion_dev *dev=devs; dev; dev=dev->next ) {
      FD_SET(dev->csk, &fds); if ( dev->csk > fdmax ) fdmax = dev->csk;
      FD_SET(dev->isk, &fds); if ( dev->isk > fdmax ) fdmax = dev->isk;
    }
    if ( select(fdmax+1,&fds,NULL,NULL,NULL) < 0 ) fatal("select");
    struct timeval tv;
    gettimeofday(&tv, NULL);
    time_t now = tv.tv_sec;
    // Incoming connection ?
    if ( csk>=0 && FD_ISSET(csk,&fds) ) {
      struct motion_dev *dev = accept_device(csk, isk);
      dev->index = next_devindex++;
      dev->next = devs;
      devs = dev;
      setup_device(dev);
    }
    // Incoming input report ?
    for ( struct motion_dev *dev=devs; dev; dev=dev->next )
      if ( FD_ISSET(dev->isk, &fds) ) {
	unsigned char report[256];
	int nr = recv(dev->isk, report, sizeof(report), 0);
	if ( nr <= 0 ) {
	  fprintf(stderr, "%d disconnected\n", dev->index);
	  close(dev->csk); close(dev->isk);
	  struct motion_dev **pdev;
	  for ( pdev=&devs; *pdev!=dev; pdev=&(*pdev)->next ) ;
	  *pdev = dev->next;
	  free(dev);
	} else {
	  if ( verbose ) dump("RECV", report, nr);
	  if ( report[0] == 0xa1 ) {
	    if ( output_binary )
	      if ( ! binary_skip ) write(1, report, nr); else --binary_skip;
	    else {
	      parse_report(dev, report+1, nr-1);
	      fflush(stdout);
	    }
	  }
	}
      }

    // User command on stdin ?
    if ( FD_ISSET(0, &fds) ) {
      char line[256];
      if ( ! fgets(line, sizeof(line), stdin) ) return 0;
      int index, rgb[3];
      if ( sscanf(line, "%d rgb %d,%d,%d",
		  &index, &rgb[0], &rgb[1], &rgb[2]) == 4 )
	for ( struct motion_dev *dev=devs; dev; dev=dev->next )
	  if ( dev->index==index && dev->type==PSMOVE ) {
	    memcpy(dev->rgb, rgb, sizeof(dev->rgb));
	    if ( ! dev->latest_refresh )
	      dev->latest_refresh = now-4;  // Force send now (see below).
	  }
    }
    
    // Periodic stuff.
    for ( struct motion_dev *dev=devs; dev; dev=dev->next )
      if ( dev->type==PSMOVE && 
	   dev->latest_refresh && now>=dev->latest_refresh+3 ) {
#if 1
	for ( int i=0; i<3; ++i )
	  if ( dev->rgb[i] > 64 ) {
	    fprintf(stderr, "Limiting brightness to 25%%\n");
	    dev->rgb[i] = 64;
	  }
#endif
	psmove_set_rgb(dev, dev->rgb);
	dev->latest_refresh =
	  (dev->rgb[0]||dev->rgb[1]||dev->rgb[2]) ? now : 0;
      }
  }

  return 0;
}