/*
 * Puts MANTA MM821 PS3 gamepad into operational mode.
 * Written by Tomasz bla Fortuna, Contact: bla at thera.be
 * Public domain
 *
 * This version works for Manta alone - as far as I tested!
 * Should you notice it works for some other gamepad feel morally
 * obliged to inform me on this fact. ;)
 *
 * Based on an idea of: lurer@gmx.net
 */

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>

#include <libusb-1.0/libusb.h>

#define VENDOR_ID 0x054c
#define PRODUCT_ID 0x0268

void show_hex(const unsigned char *buf, int len) {
	int i;
	printf("  ");
	for (i=0; i<len; i++) {
		printf("%02X ", buf[i]);
		if ((i+1)%20 == 0)
			printf("\n  ");
	}
	printf("\n");
}

void get_show_desc(struct libusb_device_handle *dev, int type, int max)
{
	unsigned char buf[1024];
	int ret;
	assert(max <= sizeof(buf));
	ret = libusb_control_transfer(dev, 
	                              LIBUSB_ENDPOINT_IN |
	                              LIBUSB_RECIPIENT_INTERFACE,
	                              LIBUSB_REQUEST_GET_DESCRIPTOR,
	                              (type << 8) | 0, 0, buf, max,
	                              5000);
	if (ret < 0) {
		printf("-- Get DESC (%d) failed\n\n", type);
	}
	else {
		printf("++ Get DESC (%d) ok (read %d):\n", type, ret);
		show_hex(buf, ret);		
		printf("\n");
	}
}

struct pad_status {
	/* Binary */
	int left_joy, right_joy;
	int select, start;

	/* Analog */
	int axis0, axis1, axis2, axis3;

	int triangle, circle, cross, square;
	int left_top, left_bottom, right_top, right_bottom;
	int up, right, down, left;
};

void pad_show(const struct pad_status *pad) 
{
	printf("AXES: %3d %3d %3d %3d DIG: select=%d start=%d JOYS: %d %d\n", 
	       pad->axis0, pad->axis1, pad->axis2, pad->axis3,
	       pad->select, pad->start, pad->left_joy, pad->right_joy);
	printf("BUTT: TOXS: %3d/%3d/%3d/%3d, DIR: %3d/%3d/%3d/%3d FRONT: %3d/%3d/%3d/%3d\n", 
	       pad->triangle, pad->circle, pad->cross, pad->square,
	       pad->up, pad->right, pad->down, pad->left,
	       pad->left_top, pad->right_top, pad->left_bottom, pad->right_bottom
		);
}

unsigned int cycle;
int pad_set(struct libusb_device_handle *dev, int led, int force_left, int force_right)
{
	int ret, transferred = 0;
	unsigned char buf[50] = {0};

	/* This are set when there's no feedback at all */
	buf[0] = 0x01;
	buf[2] = 0x96;
	buf[4] = 0x96;
	buf[30] = 0xFF;
	buf[10] = 0x02;

	buf[32] = 0x01;
	buf[34] = 0x01;

	/* This are set for feedback */
	if (force_left > 255) 
		force_left = 255;
	if (force_right > 255) 
		force_right = 255;

	buf[5] = force_left;
	/* Simulating PWM */
	cycle++;
	if (cycle > 7)
		cycle = 0;
	if (cycle < force_right / 36) {
		buf[3] = 1;
	} else {
		buf[3] = 0;
	}

	buf[10] = 0x01 | (led & (2 | 4 | 8 | 16));

	ret = libusb_interrupt_transfer(dev, 
	                                LIBUSB_ENDPOINT_OUT |
	                                LIBUSB_RECIPIENT_ENDPOINT,
	                                buf, 49, &transferred, 50000);
	return ret;
}

int pad_get_status(struct libusb_device_handle *dev, struct pad_status *pad) 
{
	unsigned char buf[255];
	int transferred;

	const int f_axis0 = 6;
	const int f_axis1 = 7;
	const int f_axis2 = 8;
	const int f_axis3 = 9;

	const int f_triangle=22;
	const int f_circle=23;
	const int f_cross=24;
	const int f_square=25;

	const int f_left_top=18;
	const int f_left_bottom=20;
    
	const int f_right_top=19;
	const int f_right_bottom=21;

	const int f_up=14;
	const int f_right=15;
	const int f_down=16;
	const int f_left=17;


	/* Byte 1 mask */
	const unsigned char
		m_select = 0x01,
		m_left_joy = 0x02,
		m_right_joy = 0x04,
		m_start = 0x08;
	const int f_mask1 = 2;

	int ret;
	ret = libusb_interrupt_transfer(dev, 
	                                LIBUSB_ENDPOINT_IN |
	                                LIBUSB_RECIPIENT_INTERFACE,
	                                buf, sizeof(buf), &transferred, 500);
	if (ret != 0) {
		return -1;
	}
	// show_hex(buf, transferred);

	pad->axis0 = buf[f_axis0];
	pad->axis1 = buf[f_axis1];
	pad->axis2 = buf[f_axis2];
	pad->axis3 = buf[f_axis3];

	pad->triangle = buf[f_triangle];
	pad->circle = buf[f_circle];
	pad->cross = buf[f_cross];
	pad->square = buf[f_square];

	pad->left_top = buf[f_left_top];
	pad->right_top = buf[f_right_top];
	pad->left_bottom = buf[f_left_bottom];
	pad->right_bottom = buf[f_right_bottom];

	pad->up = buf[f_up];
	pad->down = buf[f_down];
	pad->left = buf[f_left];
	pad->right = buf[f_right];

	pad->start = !(!(buf[f_mask1] & m_start));
	pad->select = !(!(buf[f_mask1] & m_select));
	pad->left_joy = !(!(buf[f_mask1] & m_left_joy));
	pad->right_joy = !(!(buf[f_mask1] & m_right_joy));
	
	return 0;

}

void play(struct libusb_device_handle *dev) 
{
	/* Reads data from PAD and shows some basic things */
	struct pad_status pad_status;

	unsigned char buf[1024];
	int transferred;
	int ret;
	for (;;) {
		pad_get_status(dev, &pad_status);
		pad_show(&pad_status);


		/* Right joy + triangle button */
		int force0 = pad_status.axis2 - 128;
		if (force0 < 0)
			force0 = 0;
		int force1 = pad_status.axis3 - 128;
		if (force1 < 0)
			force1 = 0;

		force0 *= 2;
		force0 += pad_status.triangle;
		force1 += pad_status.square;

		force0 += pad_status.left_top;
		force1 += pad_status.right_top;

		/* Axis 0 left - left force, right - right force */
		int tmp = pad_status.axis0 - 128;
		tmp *= 2;
		if (tmp < 0) {
			force0 += -tmp;
		} else {
			force1 += tmp;
		}


		force1 *= 2;


		/* Blink led nicely */
		int led = 0;
		if (force0 > 64 || force1 > 64) {
			led |= 0x02;
		} 
		if (force0 > 128 || force1 > 128) {
			led |= 0x04;
		} 
		if (force0 > 192 || force1 > 192) {
			led |= 0x08;
		} 
		if (force0 > 250 || force1 > 250) {
			led |= 0x10;
		} 
		pad_set(dev, led, force0, force1);
	}
}


int activate(int driver_test)
{
	int ret = -1;
	unsigned char buf[1024];
	struct libusb_device_handle *devh = NULL;

	ret = libusb_init(NULL);
	if (ret < 0) {
		printf("-- Failed to initialize libusb (%d)\n", ret);
		return 2;
	}
	printf("++ libusb initialization ok\n");

	devh = libusb_open_device_with_vid_pid(NULL, VENDOR_ID, PRODUCT_ID);
	if (devh == NULL) {
		printf("-- Failed to open device handle\n");
		return -1;
	}
	printf("++ Opened device handle\n");

	ret = libusb_detach_kernel_driver(devh, 0);   
	if (ret < 0) {
		printf("-- Failed to detach kernel driver (%d)\n", ret);
		printf("   ignoring...\n");
	}
	else
		printf("++ Detached kernel driver.\n");	

	ret = libusb_set_configuration(devh, 1);
	if (ret < 0) {
		printf("-- Failed to set configuration 1 (%d)\n", ret);
		return -1;
	} else {
		printf("++ Successfully set usb configuration 1\n");
	}

	ret = libusb_claim_interface(devh, 0);
	if (ret < 0) {
		printf("-- Failed to claim interface 0\n");
		return -1;
	} else {
		printf("++ Successfully claimed interface\n");
	}

	get_show_desc(devh, LIBUSB_DT_DEVICE, 1024);
	get_show_desc(devh, LIBUSB_DT_CONFIG, 9);
	get_show_desc(devh, LIBUSB_DT_CONFIG, 1024);

	/* Send magical control transfers */
	memset(buf, 0, sizeof(buf));
	memcpy(buf, "\x42\x0c\x00\x00", 4);
	ret = libusb_control_transfer(devh, 
                       LIBUSB_ENDPOINT_OUT | 
                       LIBUSB_REQUEST_TYPE_CLASS | 
                       LIBUSB_RECIPIENT_INTERFACE,
                0x09, /* Set report */
	        0x03f4, 0, /* f4-report id, 3-feature report */
	        buf, 4, 500);
	if (ret < 0) {
		printf("-- Unable to request report\n");
		return -1;
	} else {
		printf("++ REPORT_REQ1 success.\n");
	}

	memset(buf, 0, sizeof(buf));
	memcpy(buf,
	       "\x01\x00\x00\x00\x00\x00\x00\x00\x00"
	       "\x02", 10);
	ret = libusb_control_transfer(devh, 
                       LIBUSB_ENDPOINT_OUT | 
                       LIBUSB_REQUEST_TYPE_CLASS | 
                       LIBUSB_RECIPIENT_INTERFACE,
                0x09, /* Set report */
	        0x0201, 0, /* f4-report id, 3-feature report */
	        buf, 0x31, 500);
	if (ret < 0) {
		printf("-- Unable to request report\n");
		return -1;
	} else {
		printf("++ REPORT_REQ2 success.\n");
	}

	if (driver_test) {
		printf("== Starting libusb driver\n");
		sleep(1);
		play(devh);
	}
	
	libusb_release_interface(devh, 0);

	ret = libusb_attach_kernel_driver(devh, 0);   
	if (ret < 0) {
		printf("-- Failed to reattach kernel driver (%d)\n", ret);
		return -1;
	}
	else

		printf("++ Reattached kernel driver.\n");	

	libusb_close(devh);
	libusb_exit(NULL);
	
	printf("++ Released interface and closed handle. You controller should be in operational mode now.\n");
	
	return 0;	
}


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

	ret = getuid();
	if (ret != 0) {
		printf("This has to be executed with root rights\n");
		return 1;
	}
	
	if (argc == 2 && argv[1][0] == '1') {
		ret = activate(1);
	} else {
		printf("? You can run program with argument '1' to start a test/driver mode\n");
		ret = activate(0);
	}


	if (ret != 0) {
		printf("Something went wrong: %d\n", ret);
	}
	return ret;
}