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

#define VID_LOGITECH 0x046D
#define ARRAY_SIZE(arr) sizeof(arr)/sizeof(arr[0]) 

typedef struct {
		unsigned char cmd[8];
		char desc[64];
} ff_cmd;

static const unsigned int device_ids[] = {0xC294,	/* DF */
					  0xC298,	/* DFP */
					  0xC299,	/* G25 */
					  0xC29A,	/* DFGT */
					  0xC29B,	/* G27 */
					  0xC29C	/* WII */
					  };

static struct option long_options[] = {
        {"custom", required_argument, 0, 'c'},
        {0,        0,                 0,   0}
	};

/* Standard commands that should be accepted by all
 * Logitech wheels */
static ff_cmd std_commands[] = {
		// Clear everthing
		{ {0xFE, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, ""},
		{ {0xF3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, "Reset all effects"},

		//Test FF_AUTOCENTRE
		{ {0xFE, 0x0D, 0x07, 0x07, 0xFF, 0x00, 0x00, 0x00}, "Full autocentering"},
		{ {0xFE, 0x0D, 0x06, 0x06, 0x80, 0x00, 0x00, 0x00}, "Medium autocentering"},
		{ {0xFE, 0x0D, 0x03, 0x03, 0x40, 0x00, 0x00, 0x00}, "Weak autocentering"},
		{ {0xFE, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, "No autocentering"},

		//Test FF_CONSTANT in slot 1
		{ {0x11, 0x08, 0xFF, 0x80, 0x00, 0x00, 0x00, 0x00}, "Full left constant force"},
		{ {0x11, 0x08, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00}, "Full right constant force"},
		{ {0x11, 0x08, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00}, "No constant force"},
		{ {0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, ""},

		//Test FF_SPRING in slot 2
		{ {0x21, 0x01, 0x80, 0x80, 0x33, 0x00, 0x40, 0x00}, "Weak spring to center"},
		{ {0x21, 0x01, 0x80, 0x80, 0x66, 0x00, 0x80, 0x00}, "Medium spring to center"},
		{ {0x21, 0x01, 0x80, 0x80, 0x77, 0x00, 0xFF, 0x00}, "Strong spring to center"},
		{ {0x21, 0x01, 0x50, 0xB0, 0x77, 0x00, 0xFF, 0x00}, "Strong spring to center with deadzone"},
		{ {0x23, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, ""},

		//Test FF_FRICTION in slot 3
		{ {0x41, 0x02, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00}, "Clockwise resistance"},
		{ {0x41, 0x02, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00}, "Counter-clockwise resistance"},
		{ {0x81, 0x02, 0x03, 0x00, 0x03, 0x00, 0x00, 0x00}, "weak Resistance both ways"},
		{ {0x81, 0x02, 0x07, 0x00, 0x07, 0x00, 0x00, 0x00}, "medium Resistance both ways"},
		{ {0x81, 0x02, 0x0F, 0x00, 0x0F, 0x00, 0x00, 0x00}, "strong Resistance both ways"},
		{ {0x43, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, ""},

		//Test FF_INERTIA in slot 4
		{ {0x81, 0x02, 0x03, 0x01, 0x03, 0x01, 0x00, 0x00}, "weak 'Assistance' both ways"},
		{ {0x81, 0x02, 0x07, 0x01, 0x07, 0x01, 0x00, 0x00}, "medium 'Assistance' both ways"},
		{ {0x81, 0x02, 0x0F, 0x01, 0x0F, 0x01, 0x00, 0x00}, "strong 'Assistance' both ways"},
		{ {0x83, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, ""},

		//Test Multiple Springs all together
		{ {0x11, 0x01, 0x60, 0xFF, 0x07, 0x00, 0x30, 0x00}, "first Weak spring on left (slot 1)"},
		{ {0x21, 0x01, 0x00, 0xA0, 0x70, 0x00, 0x30, 0x00}, "plus Weak spring on right (slot 2)"},
		{ {0x41, 0x01, 0x30, 0xFF, 0x07, 0x00, 0xFF, 0x00}, "plus Strong spring on left (slot 3)"},
		{ {0x81, 0x01, 0x00, 0xD0, 0x70, 0x00, 0xFF, 0x00}, "plus Strong spring on right (slot 4)"},
		{ {0xF3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, "Everything cleared"}
};

/* Autocentering commands for Formula EX */
static ff_cmd fex_ac_commands[] = {
		//Test autocentering
		{ {0xFE, 0x03, 0x07, 0x07, 0xFF, 0x00, 0x00, 0x00}, "Full autocentering"},
		{ {0xFE, 0x03, 0x03, 0x03, 0x80, 0x00, 0x00, 0x00}, "Medium autocentering"},
		{ {0xFE, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, "No autocentering"},
};


libusb_device_handle* find_wheel()
{
	libusb_device_handle* hDev = NULL;

	for(int i = 0; i < ARRAY_SIZE(device_ids); i++) {
		hDev = libusb_open_device_with_vid_pid(NULL, VID_LOGITECH, device_ids[i]);
		if(hDev != 0) {
			printf("%s %X\n", "Found wheel - PID ", device_ids[i]);
			break;
		} 
	}

	return hDev;
}

int send_custom_command(unsigned char* cmd, libusb_device_handle* hDev)
{
	//Get access to the wheel
	int stat;
	stat = libusb_detach_kernel_driver(hDev, 0);
	if (stat < 0) {
		fprintf(stderr, "Cannot detach kernel driver\n");
		return -1;
	}

	stat = libusb_claim_interface(hDev, 0);
	if (stat < 0) {
		fprintf(stderr, "Cannot claim interface\n");
		return -1;
	}
    
	//Send command
	int transferred = 0;

	stat = libusb_interrupt_transfer(hDev, 1, cmd, sizeof(cmd),
					     &transferred, 3000);

	printf("Bytes sent: %d\n", transferred);

	if (stat < 0) {
		fprintf(stderr, "Error sending USB command");
		return -1;
	}

	//Release interface and hand control over to kernel
	stat = libusb_release_interface(hDev, 0);
	if (stat < 0) {
		fprintf(stderr, "Cannot release interface\n");
		return -1;
	}

	stat = libusb_attach_kernel_driver(hDev, 0);
        if (stat < 0) {
		fprintf(stderr, "Cannot reattach kernel driver\n");
		return -1;
	}
    
	return 0;
}

int send_command(unsigned char* cmd, libusb_device_handle* hDev)
{
	int transferred = 0;

	int stat = libusb_interrupt_transfer(hDev, 1, cmd, 7,		// note reduced length
					     &transferred, 3000);

	if (stat < 0) {
		fprintf(stderr, "Error sending USB command");
		return -1;
	}
    
	return 0;
}

int main(int argc, char** argv)
{
	char* custom_cmd_s = NULL;

	libusb_init(NULL);

	libusb_device_handle* hDev = find_wheel();
	if(hDev == NULL) {
		printf("No wheel found\n");
		libusb_exit(NULL);
		return -1;
	}

	while(optind < argc) {
		int idx = -1;
		int result = getopt_long(argc, argv, "c:",
                                         long_options, &idx);
		switch(result) {
			case 'c':
				custom_cmd_s = calloc(sizeof(char), 16);
				strcpy(custom_cmd_s, optarg);

				//Create command
				char temp[3];
				unsigned char byte;
				unsigned char custom_cmd[8];

				memset(custom_cmd, 0, sizeof(custom_cmd));
				temp[2] = '\0';
				int i = 0;
				while(i < 8) {
					memcpy(temp, custom_cmd_s+(i*2), 2);
					if(strcmp(temp, "") == 0)
						break;

					sscanf(temp, "%X", &byte);
					custom_cmd[i++] = byte;
				}

				for(int j = 0; j < 8; j++) {
					printf("%02X ", custom_cmd[j]);
				}
				
				printf("\nPress ENTER to send command\n");
				getchar();
				send_custom_command(custom_cmd, hDev);

				free(custom_cmd_s);
				libusb_exit(NULL);

				return 0;
		}
	}

	//Get access to the wheel
	int stat;
	stat = libusb_detach_kernel_driver(hDev, 0);
	if (stat < 0)
		fprintf(stderr, "Cannot detach kernel driver (error %d)\n", stat);

	stat = libusb_claim_interface(hDev, 0);
	if (stat < 0)
		fprintf(stderr, "Cannot claim interface\n");

	//Send standard commands
	for(int i = 0; i < ARRAY_SIZE(std_commands); i++) {
		send_command(std_commands[i].cmd, hDev);
		if (strlen(std_commands[i].desc)) {
			printf("%s\n%s", std_commands[i].desc, "Press ENTER to continue\n");
			getchar();
		}
	}

	printf("Test autocentering for Formula EX? (Y/N)\n");
	char c = getchar();
	if(c == 'y' || c == 'Y') {
		//Send Formula EX AC commands
		for(int i = 0; i < ARRAY_SIZE(fex_ac_commands); i++) {
			send_command(std_commands[i].cmd, hDev);
			printf("%s\n%s", fex_ac_commands[i].desc, "Press ENTER to continue\n");
			getchar();
		}
	}

	//Release interface and hand control over to kernel
	stat = libusb_release_interface(hDev, 0);
	if (stat < 0)
		fprintf(stderr, "Cannot release interface\n");

	stat = libusb_attach_kernel_driver(hDev, 0);
        if (stat < 0)
		fprintf(stderr, "Cannot reattach kernel driver\n");

	libusb_exit(NULL);
	return 0;
}