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

#include "ch375dos.h"

struct ch375 ch375_dev = {
	.io_base = 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)) {
		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_interrupt(void) {
	return ch375_poll_interrupt(&ch375_dev);
}

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

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

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

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

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

void chip_version()
{
	printf("IC Version: 0x%hx\n", ch375_get_version(&ch375_dev));
}

void chip_reg(const char *arg_reg, const char *arg_value)
{
	int reg = arg_reg ? strtoul(arg_reg, NULL, 16) : -1;
	int value = arg_value ? strtoul(arg_value, NULL, 16) : -1;

	if (reg >= 0) {
		if (value >= 0) {
			ch375_write_reg(&ch375_dev, reg, value);
			printf("Reg[%2x] = %2x\n", reg, ch375_read_reg(&ch375_dev, reg));
		} else {
			printf("Reg[%2x] = %2x\n", reg, ch375_read_reg(&ch375_dev, reg));
		}
	} else {
		for (reg = 0; reg < 0xFF; ) {
			int max = reg + 16;
			printf("Regs[%2x..%2x] =", reg, max-1);
			for (; reg < max; reg++) {
				printf(" %2x", ch375_read_reg(&ch375_dev, reg));
			}
			printf("\n");
		}
	}
}

void usb_host_off()
{
	printf("Set usb mode disabled...\n");
	ch375_set_usb_mode(&ch375_dev, CH375_USB_MODE_HOST_DISABLED);
	if (!wait_with_timeout(1, poll_test_connect) || ch375_read_data(&ch375_dev) != 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_dev, CH375_USB_MODE_HOST_RESET);
	if (!wait_with_timeout(1, poll_test_connect) || ch375_read_data(&ch375_dev) != 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_dev, CH375_USB_MODE_HOST_AUTO_SOF);
	if (!wait_with_timeout(1, poll_test_connect) || ch375_read_data(&ch375_dev) != 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(&ch375_dev);
	if (!wait_with_timeout(1, poll_test_connect)) {
		printf("Test connect timedout\n");
	}
	status = ch375_read_data(&ch375_dev);
	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, poll_interrupt)) {
		return -1;
	}

	// Get and acknowledge interrupt
	status = ch375_get_int_status(&ch375_dev);
	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_dev, CH375_RD_USB_DATA0);
	return ch375_read_data_block(&ch375_dev, buffer);
}

static bool init_disk()
{
	uint8_t status;

	printf("Disk init...\n");
	ch375_disk_init(&ch375_dev);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk init timeout\n");
	}
	status = ch375_get_int_status(&ch375_dev);
	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(&ch375_dev);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk ready timeout\n");
	}
	status = ch375_get_int_status(&ch375_dev);
	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_dev, CH375_DISK_SIZE);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk size timeout\n");
	}
	status = ch375_get_int_status(&ch375_dev);
	printf("Disk size status = 0x%hx\n", status);

	if (status == CH375_INT_USB_SUCCESS) {
		int len;

		ch375_send_command(&ch375_dev, CH375_RD_USB_DATA);

		len = ch375_read_data_block(&ch375_dev, 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 already ready\n");
	} else {
		printf("Disk not ready, so trying to init\n");

		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(&ch375_dev, lba, 1);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Disk read timeout\n");
	}
	while ((status = ch375_get_int_status(&ch375_dev) == CH375_INT_USB_DISK_READ)) {
		int len;
		ch375_send_command(&ch375_dev, CH375_RD_USB_DATA);

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

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

			ch375_send_command(&ch375_dev, 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;
}

static void disk_reads(const char *arg)
{
	unsigned long lba = 0;
	if (arg) {
		lba = strtoul(arg, NULL, 10);
	}
	disk_read(lba);
}

int disk_set_lun(int lun)
{
	uint8_t max_lun = ch375_read_reg(&ch375_dev, CH375_REG_DISK_MAX_LUN);
	uint8_t cur_lun = ch375_read_reg(&ch375_dev, CH375_REG_DISK_LUN);

	printf("Disk max LUNs = %u , cur LUN = %u\n", max_lun, cur_lun);

	if (lun >= 0) {
		ch375_write_reg(&ch375_dev, CH375_REG_DISK_LUN, lun);
		printf("Set cur LUN to %u\n", lun);
	}

	return EXIT_SUCCESS;
}

void disk_set_luns(const char *luns)
{
	disk_set_lun(luns ? atoi(luns) : -1);
}

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_dev, CH375_USB_SPEED_LOW);
#endif

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

	ch375_auto_setup(&ch375_dev);
	ch375_send_command(&ch375_dev, CH375_AUTO_SETUP);
	if (!wait_interrupt_with_timeout(100)) {
		printf("Auto setup timeout\n");
	}
	status = ch375_get_int_status(&ch375_dev);
	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_descr()
{
	usb_device_descriptor *dev_descr;
	int n;

	//ch375_set_retry(&ch375_dev, 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_dev, 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(&ch375_dev, 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(&ch375_dev, 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;
}

typedef void (*cmd_func)();
typedef void (*cmd_func1)(const char *arg1);
typedef void (*cmd_func2)(const char *arg1, const char *arg2);

struct cmd {
	const char *opt;
	cmd_func func;
	const char *arg1;
	const char *arg2;
} cmds[] = {
	{"disk", (cmd_func) disk},
	{"dread", (cmd_func)disk_reads, "[sector]"},
	{"dlun", (cmd_func)disk_set_luns, "[lun]"},
	{"dsize", (cmd_func)disk_size},
	{"dready", (cmd_func)disk_ready},

	{"auto", (cmd_func)autou},
	{"host", (cmd_func)usb_host_on},
	{"hdescr", (cmd_func)host_descr},
	{"hconn", (cmd_func)usb_host_connect},
	{"hoff", (cmd_func)usb_host_off},
	{"hreset", (cmd_func)usb_host_reset},

	{"reg", (cmd_func)chip_reg, "[reg]", "[value]"},

	{"sleep", (cmd_func)chip_sleep},
	{"reset", (cmd_func)chip_reset},
	{"int", (cmd_func)dump_int_status},
	{"vers", (cmd_func)chip_version},
};

#define NUM_COMMANDS (sizeof(cmds)/sizeof(struct cmd))

static void usage()
{
	int i, j;
	printf("Usage: utest <cmd> <args>\n");
	printf("Supported commands: \n");
	for (i = 0; i < NUM_COMMANDS; i++) {
		struct cmd *cmd = &cmds[i];
		printf(" %s", cmd->opt);
		if (cmd->arg1) printf(" %s", cmd->arg1);
		if (cmd->arg2) printf(" %s", cmd->arg2);
		printf("\n");
	}
	printf("To change IO port, set CH375_PORT env var\n");
}

static struct cmd * find_cmd(const char *arg)
{
	int i;
	for (i = 0; i < NUM_COMMANDS; i++) {
		struct cmd *cmd = &cmds[i];
		if (stricmp(cmd->opt, arg) == 0) {
			return cmd;
		}
	}
	return NULL;
}

int main(int argc, const char *argv[])
{
	const char *mode = argc > 1 ? argv[1] : 0;
	char *port = getenv("CH375_PORT");
	int rc;
	struct cmd *cmd;

	if (port) {
		unsigned long lport = strtoul(port, NULL, 16);
		ch375_dev.io_base = lport;
	}

	if (!mode || stricmp(mode, "help") == 0 || stricmp(mode, "?") == 0) {
		usage();
		return EXIT_FAILURE;
	}


	cmd = find_cmd(mode);
	if (!cmd) {
		printf("Unrecognized command: %s\n", mode);
		return EXIT_FAILURE;
	}

	printf("\"%s\", using IO port 0x%x\n", cmd->opt, ch375_dev.io_base);

	dump_int_status();

	if (!cmd->arg1) {
		cmd->func();
	} else if (cmd->arg1 && !cmd->arg2) {
		cmd_func1 func = (cmd_func1) cmd->func;
		func(argc > 2 ? argv[2] : 0);
	} else if (cmd->arg1 && cmd->arg2) {
		cmd_func2 func = (cmd_func2) cmd->func;
		func(argc > 2 ? argv[2] : 0, argc > 3 ? argv[3] : 0);
	}

	return EXIT_SUCCESS;
}