import de.ailis.usb4java.libusb.*;

import java.nio.ByteBuffer;
import java.nio.IntBuffer;


class FingerState {
	public int id = -1; // an integer that uniquely identifies the finger
	float x = 0; // normalized to be in [0,1], increasing to the right
	float y = 0; // normalized to be in [0,1], increasing downward

	void copy( FingerState fs ) {
		id = fs.id;
		x = fs.x;
		y = fs.y;
	}

	public FingerState() { }
}

class PenState {
	public static final int OUT_OF_RANGE = 0;
	public static final int HOVERING = 1;
	public static final int TOUCHING = 2;
	public int distanceState;

	public static final int PEN_TIP = 0;
	public static final int ERASER = 1;
	public int tipState;

	boolean button1;
	boolean button2;

	float x; // normalized to be in [0,1], increasing to the right
	float y; // normalized to be in [0,1], increasing downward
	float z; // distance from the screen, normalized to be in [0,1]

	float pressure; // normalized to be in [0,1]

	float elevation; // normalized to be in [-1,1]; -1 for handle pointing up and tip pointing down
	float azimuth; // normalized to be in [-1,1]; -1 for handle pointing left and tip pointing right

	void reset() {
		distanceState = OUT_OF_RANGE;
		tipState = PEN_TIP;
		button1 = button2 = false;
		x = y = 0;
		z = 1;
		pressure = 0;
		elevation = azimuth = 0;
	}

	void copy( PenState ps ) {
		distanceState = ps.distanceState;
		tipState = ps.tipState;
		button1 = ps.button1;
		button2 = ps.button2;
		x = ps.x;
		y = ps.y;
		z = ps.z;
		pressure = ps.pressure;
		elevation = ps.elevation;
		azimuth = ps.azimuth;
	}

	public PenState() {
		reset();
	}
}

class MultitouchAndPenReader {
	private static final int SMALL_TIMEOUT = 2; // in milliseconds

	private static final int MULTITOUCH_VENDOR = 1386; // Vendor: 1386(decimal) == 056a(hex) == "Wacom Co., Ltd"
	private static final int MULTITOUCH_PRODUCT_ID = 246; // product id: 00f6(hex) == 246(decimal) == "Cintiq 24HD touch (DTH-2400) touchscreen"
	private static final int MULTITOUCH_INTERFACE = 1;
	private static final int MULTITOUCH_ENDPOINT_ADDRESS = 0x83;
	private static final int MULTITOUCH_TIMEOUT = 10; // in milliseconds
	private static final int MULTITOUCH_EXPECTED_PACKET_SIZE = 62;
	private static final int MULTITOUCH_MAX_X = 5184;
	private static final int MULTITOUCH_MAX_Y = 3240;

	private static final int PEN_VENDOR = 1386; // Vendor: 1386(decimal) == 056a(hex) == "Wacom Co., Ltd"
	private static final int PEN_PRODUCT_ID = 248; // product id: 00f8(hex) == 248(decimal) == "Cintiq 24HD touch (DTH-2400) tablet"
	private static final int PEN_INTERFACE = 0;
	private static final int PEN_ENDPOINT_ADDRESS = 0x82;
	private static final int PEN_TIMEOUT = 5; // in milliseconds
	private static final int PEN_EXPECTED_PACKET_SIZE = 10;
	private static final int PEN_MAX_X = 3265;
	private static final int PEN_MAX_Y = 2050;
	private static final int PEN_MAX_Z = 41;
	private static final int PEN_MAX_PRESSURE = 255;
	private static final int PEN_MAX_ELEVATION = 127;
	private static final int PEN_MAX_AZIMUTH = 63;

	public static final boolean isVerbose = true;
	public static final boolean isVeryVerbose = false;




	private DeviceHandle multitouchDeviceHandle = null;
	private DeviceHandle penDeviceHandle = null;

	public static final int MAX_FINGERS = 10;

	// These store the most-recently-fetched state.
	// These are defined here to avoid having to re-allocated this storage each time we poll,
	// and are necessary to allow comparison with the current state to determine if anything has changed.
	//
	private FingerState [] finger_new = new FingerState[ MAX_FINGERS ];
	private int numFingers_new = 0;
	private PenState pen_new = new PenState();

	private IntBuffer oneIntBuffer = IntBuffer.allocate( 1 );
	private ByteBuffer byteBuffer_touch = ByteBuffer.allocateDirect( MULTITOUCH_EXPECTED_PACKET_SIZE );
	private ByteBuffer byteBuffer_pen = ByteBuffer.allocateDirect( PEN_EXPECTED_PACKET_SIZE ); // Making this bigger only seems to slow down things, as if the call to interruptTransfer() waits long enough for the entire buffer to fill, or until the timeout (in which case we retrieve nothing).

	// These store the current and previous states
	//
	public FingerState [] finger_old = new FingerState[ MAX_FINGERS ];
	public int numFingers_old = 0;
	public FingerState [] finger = new FingerState[ MAX_FINGERS ];
	public int numFingers = 0;
	public boolean fingers_haveChangedSinceLastPoll = false; // true if there's a difference between finger_old and finger
	public PenState pen_old = new PenState(), pen = new PenState();
	public boolean pen_hasChangedSinceLastPoll = false; // true if there's a difference between pen_old and pen

	public MultitouchAndPenReader() { }

	// Searches through the current finger state
	public int findIndexOfFingerById( int id ) {
		for ( int i = 0; i < numFingers; ++i ) {
			if ( finger[i].id == id )
				return i;
		}
		return -1;
	}

	public int findIndexOfOldFingerById( int id ) {
		for ( int i = 0; i < numFingers_old; ++i ) {
			if ( finger_old[i].id == id )
				return i;
		}
		return -1;
	}

	private void checkForError( int r, String s ) {
		if ( r < 0 ) System.out.println( "Error: " + s );
	}

	// This opens and configures the USB devices for reading
	public void initialize() {
		Context context = new Context();
		int r = LibUsb.init( context );
		checkForError( r, "Call to LibUsb.init() failed" );

		DeviceList list = new DeviceList();
		r = LibUsb.getDeviceList( context, list );
		checkForError( r, "Unable to get list of USB devices" );

		try {
			for ( Device device : list ) {
				DeviceDescriptor descriptor = new DeviceDescriptor();
				r = LibUsb.getDeviceDescriptor( device, descriptor );
				if ( r >= 0 ) {
					if ( descriptor.idVendor() == MULTITOUCH_VENDOR && descriptor.idProduct() == MULTITOUCH_PRODUCT_ID ) {
						multitouchDeviceHandle = new DeviceHandle();
						r = LibUsb.open( device, multitouchDeviceHandle );
						checkForError( r, "Unable to open device: vendor " + MULTITOUCH_VENDOR + ", product " + MULTITOUCH_PRODUCT_ID );
					}
					else if ( descriptor.idVendor() == PEN_VENDOR && descriptor.idProduct() == PEN_PRODUCT_ID ) {
						penDeviceHandle = new DeviceHandle();
						r = LibUsb.open( device, penDeviceHandle );
						checkForError( r, "Unable to open device: vendor " + PEN_VENDOR + ", product " + PEN_PRODUCT_ID );
					}
				}
			}
		}
		finally {
			LibUsb.freeDeviceList( list, true );
		}

		if ( multitouchDeviceHandle == null || penDeviceHandle == null ) {
			System.out.println( "Unable to find one or more devices" );
		}

		if ( multitouchDeviceHandle != null ) {
			// r = LibUsb.detachKernelDriver( multitouchDeviceHandle, MULTITOUCH_INTERFACE );
			r = LibUsb.setConfiguration( multitouchDeviceHandle, 1 );
			r = LibUsb.claimInterface( multitouchDeviceHandle, MULTITOUCH_INTERFACE );
			checkForError( r, "Unable to claim interface of vendor " + MULTITOUCH_VENDOR + ", product " + MULTITOUCH_PRODUCT_ID );
			ByteBuffer data = ByteBuffer.allocateDirect( 3 );
			data.put( new byte[]{18, 2, 0} );
			int bmRequestType = (0x01 << 5) | 0x01;
			int bRequest = 0x09;
			int wValue = ( 0x03 << 8 )+18;
			int wIndex = 0;
			int timeout = 1000; // in milliseconds
			r = LibUsb.controlTransfer( multitouchDeviceHandle, bmRequestType, bRequest, wValue, wIndex, data, timeout );
			checkForError( r, "Error trying to configure device of vendor " + MULTITOUCH_VENDOR + ", product " + MULTITOUCH_PRODUCT_ID );
		}
		if ( penDeviceHandle != null ) {
			// r = LibUsb.detachKernelDriver( penDeviceHandle, PEN_INTERFACE );
			r = LibUsb.setConfiguration( penDeviceHandle, 1 );
			r = LibUsb.claimInterface( penDeviceHandle, PEN_INTERFACE );
			checkForError( r, "Unable to claim interface of vendor " + PEN_VENDOR + ", product " + PEN_PRODUCT_ID );
			ByteBuffer data = ByteBuffer.allocateDirect( 2 );
			data.put( new byte[]{2, 2} );
			int bmRequestType = (0x01 << 5) | 0x01;
			int bRequest = 0x09;
			int wValue = ( 0x03 << 8 )+2;
			int wIndex = 0;
			int timeout = 1000; // in milliseconds
			r = LibUsb.controlTransfer( penDeviceHandle, bmRequestType, bRequest, wValue, wIndex, data, timeout );
			checkForError( r, "Error trying to configure device of vendor " + PEN_VENDOR + ", product " + PEN_PRODUCT_ID );
		}

		for ( int j = 0; j < MAX_FINGERS; ++j ) {
			finger_old[j] = new FingerState();
			finger[j] = new FingerState();
			finger_new[j] = new FingerState();
		}
	}

	// returns true if the fingers have changed
	public boolean pollMultitouch() {
		if ( multitouchDeviceHandle == null ) return false;

		fingers_haveChangedSinceLastPoll = false;
		boolean checkForChanges = false;

		int i;

		int r = LibUsb.interruptTransfer( multitouchDeviceHandle, MULTITOUCH_ENDPOINT_ADDRESS, byteBuffer_touch, oneIntBuffer, MULTITOUCH_TIMEOUT );
		if ( r != 0 && r != LibUsb.ERROR_TIMEOUT )
			System.out.println( "LibUsb.interruptTransfer() returned unexpected value " + r );
		int numBytesRead = ( oneIntBuffer.get(0) & 0xff ); // We "and" with 0xff to convert the byte to an unsigned value
		if ( r == LibUsb.ERROR_TIMEOUT || ( r == 0 && numBytesRead == 0 ) ) {
			numFingers_new = 0;
			checkForChanges = true;
		}
		else if ( r == 0 ) {
			if ( isVeryVerbose) System.out.println("      bytes read: " + numBytesRead );
			if ( numBytesRead == MULTITOUCH_EXPECTED_PACKET_SIZE ) {
				numFingers_new = byteBuffer_touch.get( 61 );
				if ( numFingers_new < 0 ) {
					System.out.println("Negative number of fingers");
					numFingers_new = 0;
				}
				else if ( numFingers_new > MAX_FINGERS ) {
					System.out.println("Too many fingers");
					numFingers_new = MAX_FINGERS;
				}

				final int numFingersPerPacket = 4;
				int offset = 0;
				while ( true ) {
					// The first packet will contain at most the first four fingers.
					// If there are more fingers, they will be contained in the following packets, four per packet.
					for ( i = 0; i+offset < numFingers_new && i < numFingersPerPacket; ++i ) {
						finger_new[i+offset].id = ( byteBuffer_touch.get(i*14+2) & 0xff ); // We "and" with 0xff to convert the byte to an unsigned value
						finger_new[i+offset].x = (((byteBuffer_touch.get(i*14+4)& 0xff)<<8) | (byteBuffer_touch.get(i*14+3)& 0xff)) / (float) MULTITOUCH_MAX_X;
						if ( finger_new[i+offset].x > 1 ) finger_new[i+offset].x = 1;
						finger_new[i+offset].y = (((byteBuffer_touch.get(i*14+8)& 0xff)<<8) | (byteBuffer_touch.get(i*14+7)& 0xff)) / (float) MULTITOUCH_MAX_Y;
						if ( finger_new[i+offset].y > 1 ) finger_new[i+offset].y = 1;
					}
					offset += numFingersPerPacket;
					if ( offset < numFingers_new ) {
						r = LibUsb.interruptTransfer(
							multitouchDeviceHandle, MULTITOUCH_ENDPOINT_ADDRESS,
							byteBuffer_touch, oneIntBuffer, MULTITOUCH_TIMEOUT
						);
						if ( r == 0 && oneIntBuffer.get(0) == MULTITOUCH_EXPECTED_PACKET_SIZE ) {
							// ok
						}
						else {
							System.out.println("Unexpected condition: incomplete multitouch event: returned " + r + ", " + (oneIntBuffer.get(0) & 0xff) + " bytes read");
							break;
						}
					}
					else break;
				}

				checkForChanges = true;

				// TODO XXX should we immediately try to read another packet, with SMALL_TIMEOUT, like in pollPen() ?  This might avoid dropping packets.
			}
			else {
				System.out.println( "Unexpected number of bytes read: " + numBytesRead );
			}
		}

		if ( checkForChanges ) {
			// check if things have changed since last time
			if ( numFingers_new != numFingers )
				fingers_haveChangedSinceLastPoll = true;
			else {
				for ( i = 0; i < numFingers_new; ++i ) {
					if (
						finger_new[i].id != finger[i].id
						|| finger_new[i].x != finger[i].x
						|| finger_new[i].y != finger[i].y
					) {
						fingers_haveChangedSinceLastPoll = true;
						break;
					}
				}
			}

			if ( fingers_haveChangedSinceLastPoll ) {
				// copy things over
				for ( i = 0; i < numFingers; ++i ) {
					finger_old[i].copy( finger[i] );
				}
				for ( i = 0; i < numFingers_new; ++i ) {
					finger[i].copy( finger_new[i] );
				}
				numFingers_old = numFingers;
				numFingers = numFingers_new;
			}
		}

		return fingers_haveChangedSinceLastPoll;
	}

	// returns true if the pen has changed
	public boolean pollPen() {
		if ( penDeviceHandle == null ) return false;

		pen_hasChangedSinceLastPoll = false;
		boolean checkForChanges = false;

		int r = LibUsb.interruptTransfer( penDeviceHandle, PEN_ENDPOINT_ADDRESS, byteBuffer_pen, oneIntBuffer, PEN_TIMEOUT );
		if ( r != 0 && r != LibUsb.ERROR_TIMEOUT )
			System.out.println( "LibUsb.interruptTransfer() returned unexpected value " + r );
		int numBytesRead = ( oneIntBuffer.get(0) & 0xff ); // We "and" with 0xff to convert the byte to an unsigned value
		if ( r == LibUsb.ERROR_TIMEOUT || ( r == 0 && numBytesRead == 0 ) ) {
			pen_new.copy( pen );
			pen_new.distanceState = PenState.OUT_OF_RANGE;
			pen_new.z = 1;
			checkForChanges = true;
		}
		else if ( r == 0 && numBytesRead > 0 ) {
			if ( isVeryVerbose) System.out.println("      bytes read: " + numBytesRead );
			if ( numBytesRead % PEN_EXPECTED_PACKET_SIZE == 0 ) {
				// The number of bytes to interpret is a multiple of PEN_EXPECTED_PACKET_SIZE
				checkForChanges = true;

				// For now, assume that the tip state, and other parameters, have not changed since the last event
				pen_new.copy( pen );

				while ( true ) {

					// Interpret the bytes packet-by-packet.
					// ii is an offset into byteBuffer_pen.
					for ( int ii = 0; ii < numBytesRead; ii += PEN_EXPECTED_PACKET_SIZE ) {
						boolean _button1 = ( (byteBuffer_pen.get(ii+1) & 0x2) >> 1 )==1;
						boolean _button2 = ( (byteBuffer_pen.get(ii+1) & 0x4) >> 2 )==1;

						int _x = (byteBuffer_pen.get(ii+2) & 0xFF) << 4;
						_x |= (byteBuffer_pen.get(ii+3) & 0xF0) >> 4;

						int _y = (byteBuffer_pen.get(ii+4) & 0xFF) << 4;
						_y |= (byteBuffer_pen.get(ii+5) & 0xF0) >> 4;

						int _z = (byteBuffer_pen.get(ii+9) & 0xfc) >> 2;

						int _pressure = (byteBuffer_pen.get(ii+6) & 0xff); // We "and" with 0xff to convert the byte to an unsigned value
						if ( _pressure < 0 )
							System.out.println("Unexpected condition encountered: negative pressure");

						int _elevation = byteBuffer_pen.get(ii+8) & 0x7f;
						int _azimuth = byteBuffer_pen.get(ii+7) & 0x3f;

						if ( _z == 0 ) {
							// This is a special packet, telling us which tip of the pen is in use.
							// The other information in the packet is not useful,
							// so we'll only record the useful parts
							// and assume that the rest of pen_new should remain as it was
							// with the previous packet.
							if ( _pressure == 10 && _x == 2050 && _y == 2944 ) { // pen tip approaching
								pen_new.distanceState = PenState.HOVERING;
								pen_new.tipState = PenState.PEN_TIP;
								pen_new.z = 1;
								if ( isVerbose) System.out.println("{ pen tip");
							}
							else if ( _pressure == 10 && _x == 2058 && _y == 2944 ) { // eraser approaching
								pen_new.distanceState = PenState.HOVERING;
								pen_new.tipState = PenState.ERASER;
								pen_new.z = 1;
								if ( isVerbose ) System.out.println("[ eraser");
							}
							else if ( _pressure == 0 && _x == 0 && _y == 0 ) { // leaving
								pen_new.distanceState = PenState.OUT_OF_RANGE;
								pen_new.z = 1;
								if ( isVerbose ) System.out.println((pen_new.tipState == PenState.PEN_TIP ? "}" : "]") + " stylus out of range" );
							}
							else {
								System.out.println("Unexpected condition encountered");
							}
							pen_new.pressure = 0;
						}
						else {
							// This is a normal packet.  Record all the information we can retrieve from it.
							pen_new.button1 = _button1;
							pen_new.button2 = _button2;
							pen_new.x = _x / (float) PEN_MAX_X;
							pen_new.y = _y / (float) PEN_MAX_Y;
							pen_new.z = ( _z - 21 ) / (float) PEN_MAX_Z;
							if ( pen_new.x < 0 || pen_new.y < 0 || pen_new.z < 0 || pen_new.x > 1 || pen_new.y > 1 || pen_new.z > 1 )
								System.out.println("Unexpected packet: x=" + _x + ", y=" + _y + ", z=" + _z + ", pressure=" + _pressure );
							if ( pen_new.z < 0 ) pen_new.z = 0;
							else if ( pen_new.z > 1 ) pen_new.z = 1;

							if ( pen_new.z > 0.5f && _pressure > 0 )
								System.out.println("Unexpected condition encountered (large z, positive pressure)");
							pen_new.pressure = pen_new.z > 0.5f ? 0 : ( _pressure / (float) PEN_MAX_PRESSURE );
							pen_new.distanceState = pen_new.pressure > 0 ? PenState.TOUCHING : PenState.HOVERING;
							if ( pen_new.pressure > 0 ) pen_new.z = 0;

							pen_new.elevation = _elevation / (float) PEN_MAX_ELEVATION * 2.0f - 1.0f;
							pen_new.azimuth = _azimuth / (float) PEN_MAX_AZIMUTH * 2.0f - 1.0f;
						}
					} // for

					r = LibUsb.interruptTransfer( penDeviceHandle, PEN_ENDPOINT_ADDRESS, byteBuffer_pen, oneIntBuffer, SMALL_TIMEOUT );
					numBytesRead = ( oneIntBuffer.get(0) & 0xff ); // We "and" with 0xff to convert the byte to an unsigned value
					if ( r == 0 && numBytesRead == PEN_EXPECTED_PACKET_SIZE ) {
						// ok
					}
					else {
						if ( r != 0 && r != LibUsb.ERROR_TIMEOUT )
							System.out.println( "LibUsb.interruptTransfer() returned unexpected value " + r );
						if ( numBytesRead != 0 && numBytesRead != PEN_EXPECTED_PACKET_SIZE )
							System.out.println( "LibUsb.interruptTransfer() read unexpected number of bytes " + numBytesRead );
						break;
					}

				} // while
			}
			else System.out.println( "Unexpected number of bytes read: " + numBytesRead );
		}

		if ( checkForChanges ) {
			// check if things have changed since last time
			if (
				pen_new.distanceState != pen.distanceState
				|| pen_new.tipState != pen.tipState
				|| pen_new.button1 != pen.button1
				|| pen_new.button2 != pen.button2
				|| pen_new.x != pen.x
				|| pen_new.y != pen.y
				|| pen_new.z != pen.z
				|| pen_new.pressure != pen.pressure
				|| pen_new.elevation != pen.elevation
				|| pen_new.azimuth != pen.azimuth
			) {
				pen_hasChangedSinceLastPoll = true;

				// copy things over
				pen_old.copy( pen );
				pen.copy( pen_new );
			}
		}

		return pen_hasChangedSinceLastPoll;
	}

	public void finalize() {
		LibUsb.close( multitouchDeviceHandle );
		LibUsb.close( penDeviceHandle );
	}

}