/*
 * Misc boot support
 */
#include "common.h"
#include <stdlib.h>
#include <limits.h>
#include <ctype.h>
#include <util/delay.h>
#include <avr/pgmspace.h>

#include "command.h"
#include "getopt-min.h"
#include "z80-if.h"
#include "pin.h"
#include "debug.h"

/* ugly hack to get Z180 loadfile into flash memory */
#define const const FLASH
#include "../z180/hdrom.h"
#undef const



static void z80_load_mem(void)
{
	unsigned sec = 0;
	uint32_t sec_base = hdrom_start;

	printf_P(PSTR("Loading Z180 memory... \n"));

	while (sec < hdrom_sections) {
		printf_P(PSTR("   From: 0x%.5lX to: 0x%.5lX    (%5li bytes)\n"),
				hdrom_address[sec],
				hdrom_address[sec]+hdrom_length_of_sections[sec] - 1,
				hdrom_length_of_sections[sec]);

		z80_bus_cmd(Request);
		z80_write_block((const FLASH unsigned char *) &hdrom[sec_base],  /* src */
				hdrom_address[sec],                  /* dest */
				hdrom_length_of_sections[sec]);      /* len */
		z80_bus_cmd(Release);
		sec_base+=hdrom_length_of_sections[sec];
		sec++;
	}
}

command_ret_t do_loadf(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	(void) cmdtp; (void) flag; (void) argc; (void) argv;

	if (z80_bus_state() & ZST_RUNNING) {
		printf_P(PSTR("## Can't load while CPU is running!\n"));
		return CMD_RET_FAILURE;
	}

	z80_load_mem();

	return CMD_RET_SUCCESS;
}


command_ret_t do_busreq_pulse(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	uint16_t count=1;

	(void) cmdtp; (void) flag;

	if (!(z80_bus_state() & ZST_RUNNING)) {
		printf_P(PSTR("## CPU is not running!\n"));
		return CMD_RET_FAILURE;
	}

	if (argc > 1)
		count = (uint16_t) strtoul(argv[2], NULL, 16);

	z80_bus_cmd(Request);
	while (count--)
		z80_bus_cmd(M_Cycle);

	return CMD_RET_SUCCESS;
}


command_ret_t do_go(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	uint32_t addr;

	(void) cmdtp; (void) flag;

	if (argc < 2)
		return CMD_RET_USAGE;
	addr = strtoul(argv[1], NULL, 16);
	if (addr >= (1UL<<16)) {
		printf_P(PSTR("## Startaddress 0x%05lx too high.\n"
			"   (Out of logical address space (0x00000-0x0ffff))\n"),
			addr);
		return CMD_RET_FAILURE;
	}

	if (z80_bus_state() & ZST_RUNNING) {
		printf_P(PSTR("## CPU allready running!\n"));
		return CMD_RET_FAILURE;
	}

	printf_P(PSTR("## Starting application at 0x%04lx ...\n"), addr);

	if (addr != 0) {
		uint8_t tmp[3];
		uint_fast8_t i;

		z80_bus_cmd(Request);
		for (i = 0; i < 3; i++)
			tmp[i] = z80_read(i);
		z80_write(0, 0xc3);
		z80_write(1, addr);
		z80_write(2, (addr >> 8));

		z80_bus_cmd(Run);
		z80_bus_cmd(M_Cycle);
		z80_bus_cmd(M_Cycle);
		for (i = 0; i < 3; i++)
			z80_write(i, tmp[i]);
	} else
		z80_bus_cmd(Run);

	z80_bus_cmd(Release);

	return CMD_RET_SUCCESS;
}

command_ret_t do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	(void) cmdtp; (void) flag; (void) argc; (void) argv;

	printf_P(PSTR("## CPU now in reset state.\n"));

	z80_bus_cmd(Reset);
	return CMD_RET_SUCCESS;
}

command_ret_t do_restart(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	(void) cmdtp; (void) flag; (void) argc; (void) argv;

	z80_bus_cmd(Restart);

	return CMD_RET_SUCCESS;
}

#if 0
command_ret_t do_clock(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
{
	long freq;
	char *endp;

	(void) cmdtp; (void) flag;
	
	if (argc == 2) {
		if (toupper(argv[1][0]) == 'L')
			freq = 0;
		else if (toupper(argv[1][0]) == 'H')
			freq = LONG_MAX;
		else {
			freq = strtol(argv[1], &endp, 10);
			switch (*endp) {
			case 'M':
				freq *= 1000;
			case 'K':
				freq *= 1000;
				endp++;
			case '\0':
				if (*endp == '\0')
					break;
			default:
				printf_P(PSTR("invalid value\n"));
				return CMD_RET_USAGE;
			}
			
			if (freq == 0) {
				printf_P(PSTR("CPU clock cannot be 0\n"));
				return CMD_RET_USAGE;
			}
			

/*			if (freq > (long) F_CPU / 2) {
				printf_P(PSTR("Max CPU clock freq. is: %luHz\n"), F_CPU/2);
				return CMD_RET_USAGE;
			}
*/
		}
		if (z80_clock_set(freq) < 0) {
			printf_P(PSTR("Setting CPU clock freq. to %luHz failed.\n"),
						freq);
		}
	}

	printf_P(PSTR("CPU clock: %luHz\n"), z80_clock_get());


	return CMD_RET_SUCCESS;
}

command_ret_t do_clock2(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
{
	long value;
	char *endp;
	uint8_t div_flag = 0;
	
	(void) cmdtp; (void) flag;
	
	if (argc >= 2) {
		if (argv[1][0] == '-' && argv[1][1] == 'd') {
			div_flag = 1;
			argc--;
			argv++;
		}
	}
	
	if (argc == 2) {
		if (toupper(argv[1][0]) == 'L')
			value = 0;
		else if (toupper(argv[1][0]) == 'H')
			value = LONG_MAX;
		else {
			value = strtol(argv[1], &endp, 10);
			switch (*endp) {
			case 'M':
				value *= 1000;
			case 'K':
				value *= 1000;
				endp++;
			case '\0':
				if (*endp == '\0')
					break;
			default:
				printf_P(PSTR("invalid value\n"));
				return CMD_RET_USAGE;
			}
			
			if (value == 0) {
				printf_P(PSTR("clk2 cannot be 0\n"));
				return CMD_RET_USAGE;
			}
			
			if (div_flag) {
				if (value > 256*1024L) {
					printf_P(PSTR("Max clk2 divider is: %lu\n"), 256*1024L);
					return CMD_RET_USAGE;
				}
			} else {
				if (value > (long) F_CPU / 2) {
					printf_P(PSTR("Max clk2 freq. is: %luHz\n"), F_CPU/2);
					return CMD_RET_USAGE;
				}
			}
		}
		if (div_flag ? z80_clock2_divset(value) : z80_clock2_set(value) < 0) {
			printf_P(PSTR("Setting clk2 freq. to %luHz failed.\n"),
						value);
		}
	}

	printf_P(PSTR("clk2: %luHz\n"), z80_clock2_get());


	return CMD_RET_SUCCESS;
}
#endif

// {INPUT, INPUT_PULLUP, OUTPUT, OUTPUT_TIMER} pinmode_t;


static void print_blanks(uint_fast8_t count)
{
	while(count--)
		putchar(' ');
}

static const FLASH char * const FLASH pinconf_str[] = {
			FSTR("?"),
			FSTR("Input"),
			FSTR("Pullup"),
			FSTR("Output"),
			FSTR("Clock"),
		};

static const FLASH char * const FLASH pinlevel_str[] = {
			FSTR("Low"),
			FSTR("High"),
			FSTR(""),
		};

int print_pin(int pin, int multi)
{
	int pinconf;
	const FLASH char *levelp;
	long div;

	pinconf = pin_config_get(pin);
	if (pinconf == OUTPUT_TIMER) {
		div = pin_clockdiv_get(pin);
		levelp = pinlevel_str[2];
	} else
		levelp = pinlevel_str[pin_read(pin)];

	if (multi) {
		printf_P(PSTR("%3d  "), pin);
		my_puts_P(pinconf_str[pinconf]);
		print_blanks(8 - strlen_P(pinconf_str[pinconf]));
		my_puts_P(levelp);
		print_blanks(6 - strlen_P(levelp));
		if (pinconf == OUTPUT_TIMER)
			printf_P(PSTR("%7ld   %8ld"), 
				div, F_CPU/div);
	} else {
		printf_P(PSTR("Pin %d: "), pin);
		my_puts_P(pinconf_str[pinconf]);
		printf_P(PSTR(", "));
		my_puts_P(levelp);

		if (pinconf == OUTPUT_TIMER)
			printf_P(PSTR("divide by %ld (%ldHz)"), 
				div, F_CPU/div);
	}
	printf_P(PSTR("\n"));

	return 0;
}

int pinarg_insert(int pin, int count, int pinarg[])
{
	int pos;

	if (pin < 0 || pin >= PIN_MAX)
		return -1;

	for (pos = 0; pos < count; pos++) {
		if (pin == pinarg[pos])
			return 0;
		if (pin <  pinarg[pos])
			break;
	}
	for (int i = count-1; i == pos ; i--)
		pinarg[i+1] = pinarg[i];
	pinarg[pos] = pin;

	return 1;
}

int pinarg_get(char * arg, int pinarg[])
{
	int count = 0;
	char *endp;
	int pin1, pin2, rc;

	while (1) {
		pin1 = (int) strtoul(arg, &endp, 10);
		if (endp != arg && *endp == '-') {
			arg = endp+1;
			pin2 = (int) strtoul(arg, &endp, 10);
			if (pin1 < pin2)
				for (; pin1 < pin2; pin1++)
					if ((rc = pinarg_insert(pin1, count, pinarg)) >= 0)
						count += rc;
					else
						return 0;
			else
				return 0;
		}
		if (endp != arg && pin1 >= 0) {
			if ((*endp == ',' || *endp == '\0') &&
					(rc = pinarg_insert(pin1, count, pinarg)) >= 0) {
				count += rc;
				if (*endp == '\0')
					return count;
			} else
					return 0;
		} else
			return 0;

		arg = endp+1;
	}
}


command_ret_t do_pin(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
{
	char printheader = 1;
	int pinarg[PIN_MAX];
	int pinargc;

	(void) cmdtp; (void) flag;

	/* reset getopt() */
	optind = 1;

	int opt;
	while ((opt = getopt(argc, argv, PSTR("s"))) != -1) {
		switch (opt) {
		case 's':
			printheader = 0;
			break;
		default: /* '?' */
			return CMD_RET_USAGE;
		}
	}

	/* remaining arguments */
	argc -= optind;

	if (argc == 0)
		/* print cofig of all pins */
		for (pinargc = 0; pinargc < PIN_MAX; pinargc++)
			pinarg[pinargc] = pinargc;
	else {
		/* get first arg */
		pinargc = pinarg_get(argv[optind++], pinarg);
		if (pinargc == 0)
			return CMD_RET_USAGE;
		else
			argc--;
	}

	if (argc == 0) {
		/* no more args, print config */
		if (pinargc == 1)
			print_pin(pinarg[0], 0);
		else {
			if (printheader)
				printf_P(PSTR("Pin  Config  Level  Divider  Frequency/Hz\n"
					"-----------------------------------------\n"));
			for (int i = 0; i < pinargc; i++)
				print_pin(pinarg[i], 1);
		}
		return CMD_RET_SUCCESS;
	}

	/* arguments must be in pairs: pins conf */
	if (argc % 2 != 1)
		return CMD_RET_USAGE;

	while (argc > 0) {
		char *endp;
		pinmode_t mode = NONE;
		int level = 0;
		unsigned long value = 0;
		uint8_t hz_flag = 0;
		
		switch (toupper(argv[optind][0])) {
		case 'H':
			level = 1;
		case 'L':
			mode = OUTPUT;
			break;
		case 'P':
			mode = INPUT_PULLUP;
			break;
		case 'I':
		case 'T':
			mode = INPUT;
			break;

		default:
			value = strtoul(argv[optind], &endp, 10);
			switch (*endp) {
			case 'M':
				value *= 1000;
			case 'K':
				value *= 1000;
				endp++;
			}

			if (*endp && strcmp_P(endp, PSTR("Hz")) == 0) {
				hz_flag = 1;
				endp += 2;
			}

			if (*endp != '\0') {
				printf_P(PSTR("invalid parameter: '%s'\n"), argv[optind]);
				return CMD_RET_USAGE;
			}
			
			if (value == 0) {
				printf_P(PSTR("invalid value: %lu \n"));
				return CMD_RET_USAGE;
			}

			if (hz_flag) {
				if (value > F_CPU / 2) {
					printf_P(PSTR("Max frequency is: %luHz\n"), F_CPU/2);
					return CMD_RET_USAGE;
				}
				value = F_CPU/value;
			}
			mode = OUTPUT_TIMER;

		}

		if (mode == NONE)
			return CMD_RET_USAGE;

		for (int i = 0; i < pinargc; i++) {
			switch (mode) {
			case OUTPUT:
				pin_write(pinarg[i], level);
				/* fall thru */
			case INPUT:
			case INPUT_PULLUP:
				pin_config(pinarg[i], mode);
				break;
			case OUTPUT_TIMER:
				if (pin_clockdiv_set(pinarg[i], value) < 0) {
					printf_P(PSTR("Setting pin %d to %lu failed.\n"),
								pinarg[i], value);
				}
				break;
			default:
				break;
			}
		}

		optind++;
		pinargc = pinarg_get(argv[optind++], pinarg);
		argc -= 2;
	}

	return CMD_RET_SUCCESS;
}