/*
 * 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("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;
}


/* 
 * TODO:	- pin groups 
 *		- error if pin "config clock" on pins without clock
 *		- stat for single pin (group)
 */
 
command_ret_t do_pin(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
{
	int opt, pin;
	unsigned long value;
	char *endp;
	char printheader = 1;

	(void) cmdtp; (void) flag;

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

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

//	if ((argc - optind) % 2 != 0)
//			return CMD_RET_USAGE;

	debug("argc: %d, optind: %d\n", argc, optind);

	switch (argc - optind) {
	case 0:
		if (printheader)
			printf_P(PSTR("Pin  Config  Level  Divider  Frequency/Hz\n"
				"-----------------------------------------\n"));
		for (pin = 0; pin < PIN_MAX; pin++) 
			print_pin(pin, 1);
			
		return CMD_RET_SUCCESS;
		break;
	case 1:
		pin = strtol(argv[optind], &endp, 10);
		print_pin(pin, 0);
		return CMD_RET_SUCCESS;
		break;
	}

	while (optind < argc ) {
		uint8_t hz_flag = 0;
		
		pin = strtol(argv[optind++], &endp, 10);

		switch (toupper(argv[optind][0])) {
		case 'L':
		case 'H':
			pin_write(pin, toupper(argv[optind][0]) == 'H');
			pin_config(pin, OUTPUT);
			break;
		case 'P':
			pin_config(pin, INPUT_PULLUP);
			break;
		case 'I':
		case 'T':
			pin_config(pin, 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;
			}


			debug("** setting pin '%d' to '%lu'\n", pin, value);
			if (pin_clockdiv_set(pin, value) < 0) {
				printf_P(PSTR("Setting pin %d to %lu failed.\n"),
							pin, value);
			}
		}

		optind++;
	}


	return CMD_RET_SUCCESS;
}