//
// To be able to compile on Ubuntu, you may need to first do
//    apt-get install libusb-1.0-0-dev
// to install the libusb-1.0 library (with header file).
// Then, compile with
//    g++ -o readCintiq24HDTouch readCintiq24HDTouch.cpp -lusb-1.0
//

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <libusb-1.0/libusb.h>




#if 0
#include <linux/usb/ch9.h>
#else
// These are a minimum required from the ch9.h header file
#define USB_TYPE_CLASS       (0x01 << 5)
#define USB_RECIP_INTERFACE  0x01
#endif

// I copied these from wacom_sys.c in the source code for input-wacom
#define USB_REQ_SET_REPORT 0x09
#define WAC_HID_FEATURE_REPORT 0x03




const int VENDOR = 1386; // Vendor: 1386(decimal) == 056a(hex) == "Wacom Co., Ltd"

const int MULTITOUCH_PRODUCT_ID = 246; // product id: 00f6(hex) == 246(decimal) == "Cintiq 24HD touch (DTH-2400) touchscreen"
const int MULTITOUCH_INTERFACE = 1;
const int MULTITOUCH_ENDPOINT_ADDRESS = 0x83;
const int MULTITOUCH_TIMEOUT = 7; // in milliseconds
const int MULTITOUCH_EXPECTED_PACKET_SIZE = 62; // in bytes

const int PEN_PRODUCT_ID = 248; // product id: 00f8(hex) == 248(decimal) == "Cintiq 24HD touch (DTH-2400) tablet"
const int PEN_INTERFACE = 0;
const int PEN_ENDPOINT_ADDRESS = 0x82;
const int PEN_TIMEOUT = 5; // in milliseconds
const int PEN_EXPECTED_PACKET_SIZE = 10; // in bytes



class USBDevice {
public:
   libusb_device * devicePointer;
   libusb_device_handle * deviceHandle;
   struct libusb_device_descriptor deviceDescriptor;
   unsigned int idVendor;
   unsigned int idProduct;

   USBDevice() : devicePointer(NULL), deviceHandle(NULL), idVendor(0), idProduct(0) {
   }

   void initializeDevice( libusb_device * dp ) {
      devicePointer = dp;

      int returnValue = libusb_get_device_descriptor( devicePointer, & deviceDescriptor );
      if ( returnValue != LIBUSB_SUCCESS )
         printf("Error obtaining descriptor\n");

      idVendor = deviceDescriptor.idVendor;
      idProduct = deviceDescriptor.idProduct;
   }

   void open() {
      int returnValue = libusb_open( devicePointer, & deviceHandle );
      if ( returnValue != LIBUSB_SUCCESS ) {
         printf("Error opening device -- maybe you need to run this program as root\n");
      }
   }

   void close() {
      libusb_close( deviceHandle );
      deviceHandle = NULL;
      devicePointer = NULL;
   }
};

void printBytes( unsigned char * data, int length, const char * prefix ) {
   printf( "%s", prefix );
   for ( int i = 0; i < length; ++ i ) {
      printf("%3d ", (int)(data[i] ) );
   }
   printf("\n");
}

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

   bool isVerbose = false;

   int returnValue = libusb_init( NULL );

   libusb_device ** arrayOfPointersToDevices = NULL;
   ssize_t numUSBDevices = libusb_get_device_list( NULL, & arrayOfPointersToDevices );

   if ( isVerbose )
      printf( "%lu USB devices found\n", numUSBDevices );

   USBDevice * multitouchDevice = NULL;
   USBDevice * penDevice = NULL;

   ssize_t deviceIndex = 0;
   while ( deviceIndex < numUSBDevices ) {
      if ( isVerbose )
         printf( "[device %lu]\n", deviceIndex );

      libusb_device * devicePointer = arrayOfPointersToDevices[ deviceIndex ];

      USBDevice * usbdevice = new USBDevice();
      usbdevice->initializeDevice( devicePointer );

      if ( usbdevice->idVendor == VENDOR ) {
         if ( usbdevice->idProduct == MULTITOUCH_PRODUCT_ID ) {
            puts("Found multitouch device");
            multitouchDevice = usbdevice;
            multitouchDevice->open();
            usbdevice = NULL;
         }
         else if ( usbdevice->idProduct == PEN_PRODUCT_ID ) {
            puts("Found pen device");
            penDevice = usbdevice;
            penDevice->open();
            usbdevice = NULL;
         }
      }

      // Close and try next one.
      if ( usbdevice != NULL ) {
         usbdevice->close();
         usbdevice = NULL;
      }
      deviceIndex ++;
   }

   if ( multitouchDevice == NULL )
      printf("Error: multitouch device not found (is the USB cable connected?)\n");
   if ( penDevice == NULL )
      printf("Error: pen device not found (is the USB cable connected?)\n");


   // The below code for configuring the devices was developed by studying
   // the function wacom_query_tablet_data() in wacom_sys.c in the source code for input-wacom
   // To quote the comment preceding that function,
   //    "Wacom tablets are typically configured to power-up in a mode which sends mouse-like
   //    reports to the OS. To get absolute position, pressure data, etc.
   //    from the tablet, it is necessary to switch the tablet out of this
   //    mode and into one which sends the full range of tablet data."


   if ( multitouchDevice != NULL ) {
      int returnValue = libusb_detach_kernel_driver( multitouchDevice->deviceHandle, MULTITOUCH_INTERFACE );
      printf("Result of detaching multitouchDevice's interface: %d\n", returnValue );
      // TODO should I be calling this ?
      // returnValue = libusb_set_configuration( multitouchDevice->deviceHandle, 1 );
      // printf("Result of setting multitouchDevice's configuration: %d\n", returnValue );
      returnValue = libusb_claim_interface( multitouchDevice->deviceHandle, MULTITOUCH_INTERFACE );
      printf("Result of claiming multitouchDevice's interface: %d\n", returnValue );
      unsigned char data[10] = { 18, 2, 0 };

      returnValue = libusb_control_transfer( multitouchDevice->deviceHandle,
         USB_TYPE_CLASS | USB_RECIP_INTERFACE,
         USB_REQ_SET_REPORT,
         (WAC_HID_FEATURE_REPORT<<8)+18,
         0, // in the source code for wacom_sys.c, this argument was something like bInterfaceNumber, so should I be passing MULTITOUCH_INTERFACE ?  I tried using 0 and 1, and it doesn't seem to make any difference.
         data, 3, 1000
      );
      printf("Result of configuring multitouchDevice: %d (%d means timeout, %d means pipe error, %d means no device)\n", returnValue, LIBUSB_ERROR_TIMEOUT, LIBUSB_ERROR_PIPE, LIBUSB_ERROR_NO_DEVICE );
   }

   if ( penDevice != NULL ) {
      //libusb_config_descriptor * penDevice_config_desc;
      //libusb_get_active_config_descriptor(
      //   penDevice->devicePointer, &penDevice_config_desc
      //);

      int returnValue = libusb_detach_kernel_driver( penDevice->deviceHandle, PEN_INTERFACE );
      printf("Result of detaching penDevice's interface: %d\n", returnValue );
      // TODO should I be calling this ?
      // returnValue = libusb_set_configuration( penDevice->deviceHandle, 1 );
      // printf("Result of setting penDevice's configuration: %d\n", returnValue );
      returnValue = libusb_claim_interface( penDevice->deviceHandle, PEN_INTERFACE );
      printf("Result of claiming penDevice's interface: %d\n", returnValue );
      unsigned char data[10] = { 2, 2 };
      returnValue = libusb_control_transfer( penDevice->deviceHandle,
         USB_TYPE_CLASS | USB_RECIP_INTERFACE,
         USB_REQ_SET_REPORT,
         (WAC_HID_FEATURE_REPORT<<8)+2,
         0, // in the source code for wacom_sys.c, this argument was something like bInterfaceNumber, so should I be passing PEN_INTERFACE ?
         data, 2, 1000
      );
      printf("Result of configuring penDevice: %d (%d means timeout, %d means pipe error, %d means no device)\n", returnValue, LIBUSB_ERROR_TIMEOUT, LIBUSB_ERROR_PIPE, LIBUSB_ERROR_NO_DEVICE );
   }

   int verbosityLevel = 3; // between 1 and 3
   bool readTouch = true;
   bool readPen = true;
   int count = 0;
   const int ARRAY_SIZE = 512;
   unsigned char data_touch[ ARRAY_SIZE ];
   unsigned char data_pen[ ARRAY_SIZE ];
   int bytesRead_touch=0, bytesRead_pen=0;
   int bytesRead_touch_sum = 0, bytesRead_touch_count = 0;
   int bytesRead_pen_sum = 0, bytesRead_pen_count = 0;
   bool isEraser = false;
   bool wasZGreaterThanHalf = true;
   bool wasPressurePositive = false;
   while ( true ) {
      if ( readTouch ) {
         libusb_interrupt_transfer( multitouchDevice->deviceHandle, MULTITOUCH_ENDPOINT_ADDRESS, data_touch, ARRAY_SIZE, & bytesRead_touch, MULTITOUCH_TIMEOUT );
      }
      if ( readPen ) {
         libusb_interrupt_transfer( penDevice->deviceHandle, PEN_ENDPOINT_ADDRESS, data_pen, ARRAY_SIZE, & bytesRead_pen, PEN_TIMEOUT );
      }

      if ( readTouch && bytesRead_touch > 0 ) {
         bytesRead_touch_sum += bytesRead_touch;
         bytesRead_touch_count ++;
         if ( verbosityLevel >= 2 ) printf("multitouchDevice: %d bytes read (average per attempt: %f)\n", bytesRead_touch, bytesRead_touch_sum/(float)bytesRead_touch_count );
         if ( verbosityLevel >= 3 ) printBytes( data_touch, bytesRead_touch, "raw touch data:" );
         if ( bytesRead_touch == MULTITOUCH_EXPECTED_PACKET_SIZE ) {

            unsigned char * data = data_touch;

            printf("touch: number of fingers is %d\n", (int)(data[ MULTITOUCH_EXPECTED_PACKET_SIZE - 1 ]) );

            if ( verbosityLevel >= 1 ) {
               printf("fingers(id,x,y): (%3d,%4d,%4d)   (%3d,%4d,%4d)   (%3d,%4d,%4d)   (%3d,%4d,%4d)\n",
                  (int)data[2],
                  (int)((data[   4]<<8)+data[3]),
                  (int)((data[   8]<<8)+data[7]),

                  (int)data[14+2],
                  (int)((data[14+4]<<8)+data[14+3]),
                  (int)((data[14+8]<<8)+data[14+7]),

                  (int)data[2*14+2],
                  (int)((data[2*14+4]<<8)+data[2*14+3]),
                  (int)((data[2*14+8]<<8)+data[2*14+7]),

                  (int)data[3*14+2],
                  (int)((data[3*14+4]<<8)+data[3*14+3]),
                  (int)((data[3*14+8]<<8)+data[3*14+7])
               );
            }
         }
         else if ( bytesRead_touch % MULTITOUCH_EXPECTED_PACKET_SIZE == 0 ) {
            if ( verbosityLevel >= 2 )
               printf("Received multiple touch packets; you may want to read more frequently.\n");
         }
         else {
            if ( verbosityLevel >= 2 )
               printf("Unexpected size of touch packet.");
         }
      }
      if ( readPen && bytesRead_pen > 0 ) {
         bytesRead_pen_sum += bytesRead_pen;
         bytesRead_pen_count ++;
         if ( verbosityLevel >= 2 ) printf("penDevice: %d bytes read (average per attempt: %f)\n", bytesRead_pen, bytesRead_pen_sum/(float)bytesRead_pen_count );
         if ( verbosityLevel >= 3 ) printBytes( data_pen, bytesRead_pen, "raw pen data:" );
         if ( bytesRead_pen == PEN_EXPECTED_PACKET_SIZE ) {

            unsigned char * data = data_pen;

            int button1 = (data[1] & 0x2) >> 1;
            int button2 = (data[1] & 0x4) >> 2;

            int x = (data[2] & 0xFF) << 4;
            x |= (data[3] & 0xF0) >> 4;

            int y = (data[4] & 0xFF) << 4;
            y |= (data[5] & 0xF0) >> 4;

            int pressure = data[6];
            int elevation = ( data[8] & 0x7f );
            int azimuth = ( data[7] & 0x3f );
            int distanceFromScreen = (data[9] & 0xfc) >> 2;
            if ( distanceFromScreen == 0 ) {
               if ( pressure == 10 && x==2050 && y==2944 ) {
                  printf("{ stylus pen tip approaching\n");
                  isEraser = false;
               }
               else if ( pressure == 10 && x==2058 && y==2944 ) {
                  printf("[ stylus eraser approaching\n");
                  isEraser = true;
               }
               else if ( pressure == 0 && x == 0 && y == 0 ) {
                  printf( "%s stylus out of range\n", isEraser ? "]" : "}");
               }
               else {
                  printf("Unexpected condition encountered with pen data\n");
               }
            }
            else {
               bool isPressurePositive = ( pressure > 0 );
               bool didPressureChange = false;
               char buffer1[20] = { '\0' };
               if ( wasPressurePositive != isPressurePositive ) {
                  wasPressurePositive = isPressurePositive;
                  didPressureChange = true;
                  sprintf( buffer1, "pressure %s 0; ", isPressurePositive ? ">" : "==" );
               }

               bool isZGreaterThanHalf = (distanceFromScreen-21) > 20;
               bool didZChange = false;
               char buffer2[20] = { '\0' };
               if ( wasZGreaterThanHalf != isZGreaterThanHalf ) {
                  wasZGreaterThanHalf = isZGreaterThanHalf;
                  didZChange = true;
                  sprintf(buffer2, "z %s 50%%", isZGreaterThanHalf ? ">" : "<" );
               }

               if ( didPressureChange || didZChange ) {
                  printf( "   %s%s\n", buffer1, buffer2 );
               }
               printf("      pen:  x: %4d, y: %4d, pressure: %3d, elevation: %3d, azimuth: %3d, distanceFromScreen: %3d, buttons: %d%d, eraser?: %s\n", x, y, pressure, elevation, azimuth, distanceFromScreen, button1, button2, isEraser?"yes":"no"  );
            }
         }
         else if ( bytesRead_pen % PEN_EXPECTED_PACKET_SIZE == 0 ) {
            if ( verbosityLevel >= 2 )
               printf("Received multiple pen packets; you may want to read more frequently.\n");
         }
         else {
            if ( verbosityLevel >= 2 )
               printf("Unexpected size of pen packet.");
         }
      }

      count ++;
      //printf( "                                           %d\n", count );
   }

   if ( multitouchDevice != NULL )
      multitouchDevice->close();
   if ( penDevice != NULL )
      penDevice->close();


   libusb_exit( NULL );

   printf( "Done\n" );
   return 0;
}