#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <dos.h>

#include "usb.h"
#include "ch375.h"

unsigned short ch375_port = 0x260;

#define SWAPNC_32(w)    (\
                            (((w) & 0x000000FFUL) << 24) |\
                            (((w) & 0x0000FF00UL) << 8) |\
                            (((w) & 0x00FF0000UL) >> 8) |\
                            (((w) & 0xFF000000UL) >> 24)\
                        )

static uint8_t buffer[64];

typedef uint16_t Centisecs;
#define CENTISECS_MAX (60 * 100U)

/// Gets the current wallclock centisecond (1/100 of a second).
//  Note this wraps around every minute, so max value is CENTISECS_MAX.
static Centisecs get_centisecs()
{
	struct dostime_t now;
	_dos_gettime(&now);
	return (now.second * 100U) + now.hsecond;
}

static inline Centisecs centisecs_remaining(Centisecs start, Centisecs deadline)
{
	Centisecs now = get_centisecs();
	if (now < start) {
		// Rollover happened
		now += CENTISECS_MAX;
	}
	if (now >= deadline) {
		return 0;
	} else {
		return deadline - now;
	}
}

static void delay_centisecs(Centisecs timeout)
{
	Centisecs start = get_centisecs();
	Centisecs deadline = start + timeout;
	while (centisecs_remaining(start, deadline)) {
		printf("Delay deadline=%u now=%u\n", deadline, get_centisecs());
	}
}


typedef bool (*PollFunction)(void);

static bool wait_with_timeout(Centisecs timeout, PollFunction pf) {
	Centisecs start = get_centisecs();
	Centisecs deadline = start + timeout;
	Centisecs last_print = 0;
	while (!pf() && centisecs_remaining(start, deadline)) {
		ch375_iodelay();
		if (last_print != get_centisecs()) {
			printf("Wait with timeout deadline=%u now=%u\n", deadline, get_centisecs());
			last_print = get_centisecs();
		}
	}
	if (pf()) {
		printf("Wait OK, %d centisecs elapsed\n", get_centisecs() - start);
		return true;
	} else {
		printf("Wait timeout\n");
		return false;
	}
	//return pf();
}

static bool poll_test_connect(void) {
	return ch375_read_data() != 0;
}


static bool wait_interrupt_with_timeout(unsigned int timeout) {
	return wait_with_timeout(timeout, ch375_poll_interrupt);
}

static void dump_int_status()
{
	char i = ch375_poll_interrupt() ? 'I' : ' ';
	printf("Int Status: [%c] 0x%hx\n", i, ch375_get_int_status());
	// Above also clears pending ints...
}

void chip_reset()
{
	printf("Chip reset\n");
	ch375_reset();
	delay_centisecs(5);
}

void chip_sleep()
{
	printf("Chip sleep\n");
	ch375_enter_sleep();
}

void usb_host_off()
{
	printf("Set usb mode disabled...\n");
	ch375_set_usb_mode(CH375_USB_MODE_HOST_DISABLED);
	if (!wait_with_timeout(1, poll_test_connect) || ch375_read_data() != CH375_RET_SUCESS) {
		printf("Set usb mode failed\n");
	}
}

void usb_host_reset(void)
{
	printf("Set usb mode reset...\n");
	ch375_set_usb_mode(CH375_USB_MODE_HOST_RESET);
	if (!wait_with_timeout(1, poll_test_connect) || ch375_read_data() != CH375_RET_SUCESS) {
		printf("Set usb mode reset failed\n");
	}
}

bool usb_host_on(void)
{
	printf("Set usb mode...\n");
	ch375_set_usb_mode(CH375_USB_MODE_HOST_AUTO_SOF);
	if (!wait_with_timeout(1, poll_test_connect) || ch375_read_data() != CH375_RET_SUCESS) {
		printf("Set usb mode failed\n");
		return false;
	}

	dump_int_status();

	return true;
}

bool usb_host_connect(void)
{
	uint8_t status;

	printf("Test connect...\n");
	ch375_test_connect();
	if (!wait_with_timeout(1, poll_test_connect)) {
		printf("Test connect timedout\n");
	}
	status = ch375_read_data();
	printf("Test connect status = 0x%hx\n", status);
	switch (status) {
	case CH375_INT_USB_CONNECT:
		printf("USB device connected\n");
		return true;
	case CH375_INT_USB_READY:
		printf("USB device ready\n");
		return true;
	case CH375_INT_USB_DISCONNECT:
		printf("USB device not connected\n");
	default:
		return false;
	}
}

static int usb_host_wait_int_read_data(Centisecs timeout, uint8_t buffer[])
{
	uint8_t status;
	if (!wait_with_timeout(timeout, ch375_poll_interrupt)) {
		return -1;
	}

	// Get and acknowledge interrupt
	status = ch375_get_int_status();
	printf("Wait int read data status=0x%hx\n", status);
	if (status != CH375_INT_USB_SUCCESS) {
		return -status;
	}


	// USB host mode doesn't need to do unlock, so we can use DATA0 here?
	ch375_send_command(CH375_RD_USB_DATA0);
	return ch375_read_data_block(buffer);
}

static bool init_disk()
{
	uint8_t status;

	printf("Disk init...\n");
	ch375_disk_init();
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk init timeout\n");
	}
	status = ch375_get_int_status();
	printf("Disk init status = 0x%hx\n", status);

	return status == CH375_INT_USB_SUCCESS;
}

static bool disk_ready()
{
	uint8_t status;
	ch375_test_disk_ready();
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk ready timeout\n");
	}
	status = ch375_get_int_status();
	printf("Disk ready status = 0x%hx\n", status);
	return status == CH375_INT_USB_SUCCESS;
}

static void disk_size()
{
	uint8_t status;

	printf("Disk size...\n");
	ch375_send_command(CH375_DISK_SIZE);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk size timeout\n");
	}
	status = ch375_get_int_status();
	printf("Disk size status = 0x%hx\n", status);

	if (status == CH375_INT_USB_SUCCESS) {
		int len;

		ch375_send_command(CH375_RD_USB_DATA);

		len = ch375_read_data_block(buffer);
		printf("Read %d bytes\n", len);

		if (len >= 8) {
			uint32_t sectors = SWAPNC_32(*(uint32_t*)&buffer[0]),
			         bytes_per_sector = SWAPNC_32(*(uint32_t*)&buffer[4]);
			uint32_t kbytes = sectors / 2;
			uint32_t mbytes = kbytes / 1024;

			printf("%hx %hx %hx %hx %hx %hx %hx %hx\n",
			       buffer[0], buffer[1], buffer[2], buffer[3],
			        buffer[4], buffer[5], buffer[6], buffer[7]);
			printf("%lu sectors, %lu bytes per sector, %u MiB\n", sectors, bytes_per_sector, mbytes);
		}
	}
}

int disk()
{
	bool ready = disk_ready();
	int i = 0;

	if (ready) {
		printf("Disk ready\n");
	} else {
		printf("Disk not ready\n");

		if (!usb_host_connect()) {
			return EXIT_FAILURE;
		}

		if (!init_disk()) {
			return EXIT_FAILURE;
		}

		i = 3;
		while (i >= 0 && !(ready = disk_ready())) {
			i--;
			printf("Give more time..\n");
		}

		if (ready) {
			printf("Disk now ready\n");
		} else {
			printf("Disk NOT ready\n");
			return EXIT_FAILURE;
		}
	}

	dump_int_status();

	disk_size();

	return EXIT_SUCCESS;
}

int disk_read(uint32_t lba)
{
	uint8_t status;
	unsigned total = 0;
	unsigned i;
	ch375_disk_read(lba, 1);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk read timeout\n");
	}
	while ((status = ch375_get_int_status() == CH375_INT_USB_DISK_READ)) {
		int len;
		ch375_send_command(CH375_RD_USB_DATA);

		if (total >= 512) {
			printf("Read buffer overflow!");
			break;
		}

		len = ch375_read_data_block(&buffer[total]);
		printf("Disk read data block len = %d\n", len);
		if (len > 0) {
			total += len;

			ch375_send_command(CH375_DISK_RD_GO);
			if (!wait_interrupt_with_timeout(100)) {
				printf("Disk read more timeout\n");
			}
		} else if (len == 0) {
			break;
		}
	}
	printf("Disk read status = 0x%hx\n", status);

	printf("Disk read total bytes = %u\n", total);

	for (i = 512 - (8*4); i < total; i+=8) {
		printf("%hx %hx %hx %hx  %hx %hx %hx %hx\n",
		       buffer[i+0], buffer[i+1], buffer[i+2], buffer[i+3],
		        buffer[i+4], buffer[i+5], buffer[i+6], buffer[i+7]);
	}

	return EXIT_SUCCESS;
}

int autou()
{
	uint8_t status;

#if 1
	usb_host_reset();
	delay(100);
#endif

#if 1
	if (!usb_host_on()) {
		return EXIT_FAILURE;
	}
	delay(100);
#endif

#if 1
		// TODO: Try to get descriptor in high speed, then retry in low speed?
		ch375_set_usb_speed(CH375_USB_SPEED_LOW);
#endif

#if 1
	printf("Auto setup...\n");

	ch375_auto_setup();
	ch375_send_command(CH375_AUTO_SETUP);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Auto setup timeout\n");
	}
	status = ch375_get_int_status();
	printf("Auto setup status = 0x%hx\n", status);

	delay(100);
#endif

	if (status == CH375_INT_USB_SUCCESS) {
		printf("Auto setup success\n");
	} else {
		printf("Auto setup fail, status=0x%x\n", status);
	}

	dump_int_status();

	return EXIT_SUCCESS;
}

int host()
{
	uint8_t status;
	usb_device_descriptor *dev_descr;
	int n;

	//ch375_set_retry(CH375_RETRY_NAK_INFINITE | CH375_RETRY_TIMEOUT_MAX);

#if 0
	usb_host_reset();
	//delay(100);
#endif

#if 1
	if (!usb_host_on()) {

		return EXIT_FAILURE;
	}
	//delay(100);
#endif

#if 1
		// TODO: Try to get descriptor in high speed, then retry in low speed?
		ch375_set_usb_speed(CH375_USB_SPEED_LOW);
#endif

#if 0
	if (!usb_host_connect()) {
		return EXIT_FAILURE;
	}
#endif

	dump_int_status();

	printf("Reading descriptor %d...\n", USB_DESCRIPTOR_TYPE_DEVICE);
	ch375_get_descriptor(USB_DESCRIPTOR_TYPE_DEVICE);
	n = usb_host_wait_int_read_data(100, buffer);
	if (n >= 0) {
		printf("Got descriptor 1 size = %d\n", n);
	} else {
		printf("Failed to get descriptor 1, err=0x%hx\n", -n);
		return EXIT_FAILURE;
	}

	dump_int_status();

	dev_descr = (usb_device_descriptor*)buffer;

	printf("Got USB device VID:PID %04x:%04x\n", dev_descr->idVendor, dev_descr->idProduct);

	printf("Reading descriptor %d...\n", USB_DESCRIPTOR_TYPE_CONFIG);
	ch375_get_descriptor(USB_DESCRIPTOR_TYPE_CONFIG);
	n = usb_host_wait_int_read_data(100, buffer);
	if (n >= 0) {
		printf("Got descriptor 2 size = %d\n", n);
	} else {
		printf("Failed to get descriptor 2, err=0x%hx\n", -n);
		return EXIT_FAILURE;
	}

	dump_int_status();

	return EXIT_SUCCESS;
}

int main(int argc, const char *argv[])
{
	char mode = 'd';

	if (argc > 1) {
		mode = argv[1][0];
	}

	printf("Test mode %c\n", mode);

	dump_int_status();

	switch (mode) {
	case 'd':
		return disk();
	case 'e':
		return disk_read(0);
	case 'a':
		return autou();
	case 'm':
		return host();
	case 'h':
		usb_host_on();
		return EXIT_SUCCESS;
	case 'c':
		usb_host_connect();
		return EXIT_SUCCESS;
	case 'o':
		usb_host_off();
		return EXIT_SUCCESS;
	case 'b':
		usb_host_reset();
		return EXIT_SUCCESS;
	case 's':
		chip_sleep();
		return EXIT_SUCCESS;
	case 'r':
		chip_reset();
		return EXIT_SUCCESS;
	case 'i':
		dump_int_status();
		return EXIT_SUCCESS;
	case 'v':
		printf("IC Version: 0x%hx\n", ch375_get_version());
		return EXIT_SUCCESS;
	default:
		printf("Unknown mode %c\n", mode);
		return EXIT_FAILURE;
	}
}