#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <avr/io.h>
#include <avr/interrupt.h>

#include <util/delay.h>

#include "usart1.h"
#include "usb.h"
#include "gamepad.h"
#include "gcn64_protocol.h"
#include "n64.h"

#ifndef ARRAY_SIZE
#define ARRAY_SIZE(arr)	(sizeof(arr) / sizeof(arr[0]))
#endif

#ifndef LPSTR
#define LPSTR(s)	((const PROGMEM wchar_t*)(s))
#endif

uint16_t hid_get_report(struct usb_request *rq, const uint8_t **dat);
uint8_t hid_set_report(const struct usb_request *rq, const uint8_t *dat, uint16_t len);

#include "reportdesc.c"

const wchar_t *const g_usb_strings[] = {
	[0] = L"raphnet technologies", 	// 1 : Vendor
	[1] = L"GC/N64 to USB v3.0",	// 2: Product
	[2] = L"123456",				// 3 : Serial
};

struct cfg0 {
	struct usb_configuration_descriptor configdesc;
	struct usb_interface_descriptor interface;
	struct usb_hid_descriptor hid;
	struct usb_endpoint_descriptor ep1_in;
};

static const struct cfg0 cfg0 PROGMEM = {
	.configdesc = {
		.bLength = sizeof(struct usb_configuration_descriptor),
		.bDescriptorType = CONFIGURATION_DESCRIPTOR,
		.wTotalLength = sizeof(cfg0), // includes all descriptors returned together
		.bNumInterfaces = 1,
		.bConfigurationValue = 1,
		.bmAttributes = CFG_DESC_ATTR_RESERVED, // set Self-powred and remote-wakeup here if needed.
		.bMaxPower = 25, // for 50mA
	},
	.interface = {
		.bLength = sizeof(struct usb_interface_descriptor),
		.bDescriptorType = INTERFACE_DESCRIPTOR,
		.bInterfaceNumber = 0,
		.bAlternateSetting = 0,
		.bNumEndpoints = 1,
		.bInterfaceClass = USB_DEVICE_CLASS_HID,
		.bInterfaceSubClass = HID_SUBCLASS_NONE,
		.bInterfaceProtocol = HID_PROTOCOL_NONE,
	},
	.hid = {
		.bLength = sizeof(struct usb_hid_descriptor),
		.bDescriptorType = HID_DESCRIPTOR,
		.bcdHid = 0x0101,
		.bCountryCode = HID_COUNTRY_NOT_SUPPORTED,
		.bNumDescriptors = 1, // Only a report descriptor
		.bClassDescriptorType = REPORT_DESCRIPTOR,
		.wClassDescriptorLength = sizeof(gcn64_usbHidReportDescriptor),
	},
	.ep1_in = {
		.bLength = sizeof(struct usb_endpoint_descriptor),
		.bDescriptorType = ENDPOINT_DESCRIPTOR,
		.bEndpointAddress = USB_RQT_DEVICE_TO_HOST | 1, // 0x81
		.bmAttributes = TRANSFER_TYPE_INT,
		.wMaxPacketsize = 64,
		.bInterval = LS_FS_INTERVAL_MS(1),
	},
};

const struct usb_device_descriptor device_descriptor PROGMEM = {
	.bLength = sizeof(struct usb_device_descriptor),
	.bDescriptorType = DEVICE_DESCRIPTOR,
	.bcdUSB = 0x0101,
	.bDeviceClass = 0, // set at interface
	.bDeviceSubClass = 0, // set at interface
	.bDeviceProtocol = 0,
	.bMaxPacketSize = 64,
	.idVendor = 0x289B,
	.idProduct = 0x0017,
	.bcdDevice = 0x0300, // 1.0.0
	.bNumConfigurations = 1,
	.iManufacturer = 1,
	.iProduct = 2,
	.iSerialNumber = 3,
};

static struct usb_parameters usb_params = {
	.flags = USB_PARAM_FLAG_CONFDESC_PROGMEM |
				USB_PARAM_FLAG_DEVDESC_PROGMEM |
					USB_PARAM_FLAG_REPORTDESC_PROGMEM,
	.devdesc = (PGM_VOID_P)&device_descriptor,
	.configdesc = (PGM_VOID_P)&cfg0,
	.configdesc_ttllen = sizeof(cfg0),
	.num_strings = ARRAY_SIZE(g_usb_strings),
	.strings = g_usb_strings,
	.reportdesc = gcn64_usbHidReportDescriptor,
	.reportdesc_len = sizeof(gcn64_usbHidReportDescriptor),
	.getReport = hid_get_report,
	.setReport = hid_set_report,
};

void hwinit(void)
{
	/* PORTB
	 *
	 * 7: NC    Output low
	 * 6: NC	Output low
	 * 5: NC	Output low
	 * 4: NC	Output low
	 * 3: MISO	Output low
	 * 2: MOSI	Output low
	 * 1: SCK	Output low
	 * 0: NC	Output low
	 */
	PORTB = 0x00;
	DDRB = 0xFF;

	/* PORTC
	 *
	 * 7: NC	Output low
	 * 6: NC	Output low
	 * 5: NC	Output low
	 * 4: NC	Output low
	 * 3: (no such pin)
	 * 2: NC	Output low
	 * 1: RESET	(N/A: Reset input per fuses)
	 * 0: XTAL2	(N/A: Crystal oscillator)
	 */
	PORTB = 0x00;
	DDRB = 0xff;

	/* PORTD
	 *
	 * 7: HWB		Input (external pull-up)
	 * 6: NC		Output low
	 * 5: NC		Output low
	 * 4: NC		Output low
	 * 3: IO3_MCU	Input
	 * 2: IO2_MCU	Input
	 * 1: IO1_MCU	Input
	 * 0: IO0_MCU	Input
	 */
	PORTD = 0x00;
	DDRD = 0x70;

	// System clock. External crystal is 16 Mhz and we want
	// to run at max. speed.
	CLKPR = 0x80;
	CLKPR = 0x0; // Division factor of 1
	PRR0 = 0;
	PRR1 = 0;
}

static unsigned char hid_report_data[32];
static unsigned char gamepad_report0[32];

// Output Report IDs for various functions
#define REPORT_SET_EFFECT			0x01
#define	REPORT_SET_PERIODIC			0x04
#define REPORT_SET_CONSTANT_FORCE	0x05
#define REPORT_EFFECT_OPERATION		0x0A
#define REPORT_DISABLE_ACTUATORS	0x0C
#define REPORT_PID_POOL				0x0D

// Feature reports
#define REPORT_CREATE_EFFECT		0x09

// For the 'Usage Effect Operation' report
#define EFFECT_OP_START			1
#define EFFECT_OP_START_SOLO	2
#define EFFECT_OP_STOP			3

// Feature report
#define PID_SIMULTANEOUS_MAX	3
#define PID_BLOCK_LOAD_REPORT	2

static volatile unsigned char gamepad_vibrate = 0; // output

static unsigned char vibration_on = 0;
static unsigned char constant_force = 0;
static unsigned char magnitude = 0;

static unsigned char _FFB_effect_index;
#define LOOP_MAX    0xFFFF
static unsigned int _loop_count;

static void effect_loop()
{
	if (_loop_count) {
		if (_loop_count != LOOP_MAX) {
			_loop_count--;
		}
	}
}

static void decideVibration(void)
{
	if (!_loop_count)
		vibration_on = 0;

	if (!vibration_on) {
		gamepad_vibrate = 0;
	} else {
		if (constant_force > 0x7f) {
			gamepad_vibrate = 1;
		}
		if (magnitude > 0x7f) {
			gamepad_vibrate = 1;
		}
	}
}

uint16_t hid_get_report(struct usb_request *rq, const uint8_t **dat)
{
	uint8_t report_id = (rq->wValue & 0xff);

	// USB HID 1.11 section 7.2.1 Get_Report
	// wValue high byte : report type
	// wValue low byte : report id
	switch (rq->wValue >> 8)
	{
		case HID_REPORT_TYPE_INPUT:
			{
				if (report_id == 1) { // Joystick
					// report_id = rq->wValue & 0xff
					// interface = rq->wIndex
					*dat = gamepad_report0;
					printf("Get joy report\r\n");
					return 9;
				} else if (report_id == 2) { // 2 : ES playing
					hid_report_data[0] = report_id;
					hid_report_data[1] = 0;
					hid_report_data[2] = _FFB_effect_index;
					printf("ES playing\r\n");
					*dat = hid_report_data;
					return 3;
				} else {
					printf("Get input report %d ??\r\n", rq->wValue & 0xff);
				}
			}
			break;

		case HID_REPORT_TYPE_FEATURE:
			if (report_id == PID_BLOCK_LOAD_REPORT) {
				hid_report_data[0] = report_id;
				hid_report_data[1] = 0x1;
				hid_report_data[2] = 0x1;
				hid_report_data[3] = 10;
				hid_report_data[4] = 10;
				printf("block load\r\n");
				*dat = hid_report_data;
				return 5;
			}
			else if (report_id == PID_SIMULTANEOUS_MAX) {
				hid_report_data[0] = report_id;
				// ROM Effect Block count
				hid_report_data[1] = 0x1;
				hid_report_data[2] = 0x1;
				// PID pool move report?
				hid_report_data[3] = 0xff;
				hid_report_data[4] = 1;
				printf("simultaneous max\r\n");
				*dat = hid_report_data;
				return 5;
			}
			else if (report_id == REPORT_CREATE_EFFECT) {
				hid_report_data[0] = report_id;
				hid_report_data[1] = 1;
				printf("create effect\r\n");
				*dat = hid_report_data;
				return 2;
			} else {
				printf("Unknown feature %d\r\n", rq->wValue & 0xff);
			}
			break;
	}

	printf("Unhandled hid get report type=0x%02x, rq=0x%02x, wVal=0x%04x, wLen=0x%04x\r\n", rq->bmRequestType, rq->bRequest, rq->wValue, rq->wLength);
	return 0;
}

uint8_t hid_set_report(const struct usb_request *rq, const uint8_t *data, uint16_t len)
{
	if (len < 1) {
		printf("shrt\n");
		return -1;
	}

	if ((rq->wValue >> 8) == HID_REPORT_TYPE_OUTPUT) {

		switch(data[0])
		{
			case REPORT_DISABLE_ACTUATORS:
				printf("disable actuators\r\n");
				break;
			case REPORT_PID_POOL:
				printf("pid pool\r\n");
				break;
			case REPORT_SET_EFFECT:
				_FFB_effect_index = data[1];
				printf("set effect %d\n", data[1]);
				break;
			case REPORT_SET_PERIODIC:
				magnitude = data[2];
				decideVibration();
				printf("periodic mag: %d", data[2]);
				break;
			case REPORT_SET_CONSTANT_FORCE:
				if (data[1] == 1) {
					constant_force = data[2];
					decideVibration();
					printf("Constant force");
				}
				break;
			case REPORT_EFFECT_OPERATION:
				if (len != 4)
					return -1;
				printf("EFFECT OP\n");
				/* Byte 0 : report ID
				 * Byte 1 : bit 7=rom flag, bits 6-0=effect block index
				 * Byte 2 : Effect operation
				 * Byte 3 : Loop count */
				_loop_count = data[3]<<3;

				switch(data[1] & 0x7F) // Effect block index
				{
					case 1: // constant force
					case 3: // square
					case 4: // sine
						switch (data[2]) // effect operation
						{
							case EFFECT_OP_START:
								vibration_on = 1;
								decideVibration();
								break;

							case EFFECT_OP_START_SOLO:
								vibration_on = 1;
								decideVibration();
								break;

							case EFFECT_OP_STOP:
								vibration_on = 0;
								decideVibration();
								break;
						}
						break;

					// TODO : should probably drop these from the descriptor since they are

					case 2: // ramp
					case 5: // triangle
					case 6: // sawtooth up
					case 7: // sawtooth down
					case 8: // spring
					case 9: // damper
					case 10: // inertia
					case 11: // friction
					case 12: // custom force data
						printf("Ununsed effect %d\n", data[1] & 0x7F);
						break;
				}
				break;
			default:
				printf("Set output report 0x%02x\r\n", data[0]);
		}
	}
	else if ((rq->wValue >> 8) == HID_REPORT_TYPE_FEATURE) {
		switch(data[0])
		{
			case REPORT_CREATE_EFFECT:
				_FFB_effect_index = data[1];
				printf("create effect %d\n", data[1]);
				break;

			default:
				printf("What?\n");
		}
	}
	else {
		printf("impossible\n");
	}
	return 0;
}

int main(void)
{
	Gamepad *pad = NULL;

	hwinit();
	usart1_init();
	gcn64protocol_hwinit();

	pad = n64GetGamepad();

	sei();
	usb_init(&usb_params);

	while (1)
	{
		static char last_v = 0;

		usb_doTasks();
		_delay_ms(5);
		effect_loop();
		decideVibration();

		if (last_v != gamepad_vibrate) {
			if (pad->setVibration) {
				pad->setVibration(gamepad_vibrate);
			}
			last_v = gamepad_vibrate;
		}

		pad->update();
		if (pad->changed(1)) {
			int report_size;

			report_size = pad->buildReport(gamepad_report0, 1);
			usb_interruptSend(gamepad_report0, report_size);
		}
	}

	return 0;
}