#include "common.h"
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <avr/eeprom.h>

#include "command.h"
#include "debug.h"

/*
 * Debugging
 */
#ifdef DEBUG

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

static uint8_t ram_read_byte(const uint8_t *p)
{
	return *p;
}

void dump_mem(const uint8_t *startaddr, int len, 
		uint8_t (*readfkt)(const uint8_t *), char *title)
{
	uint8_t buf[16];
	char *indent = NULL;
	uint8_t llen = 16;
	uint8_t pre = (size_t) startaddr % 16;
	const uint8_t *addr = (uint8_t *) ((size_t) startaddr & ~0x0f);
	len += pre;
	uint8_t i;
	
	if (title && *title) {
		printf_P(PSTR("%s\n"),title);
		indent = "    ";
	}
		
	while (len) {
		if (len < 16)
			llen = len;

		for (i = pre; i < llen; i++)
			buf[i] = readfkt(addr + i);

		printf_P(PSTR("%s%04x:"),indent, addr);
		for (i = 0; i < llen; i++) {
			if ((i % 8) == 0)
				putchar(' ');
			if (i < pre)
				printf_P(PSTR(".. "));
			else
				printf_P(PSTR("%.2x "), buf[i]);
		}
		/* fill line with whitespace for nice ASCII print */
		print_blanks(3 * (16u - i) + (16u-i)/8 + 1 + pre);
		/* Print data in ASCII characters */
		for (i = pre; i < llen; i++)
			printf_P(PSTR("%c"), isprint(buf[i]) ? buf[i] : '.');
		putchar('\n');

		pre = 0;
		addr += 16;
		len -= llen;
	}
}

void dump_eep(const uint8_t *addr, unsigned int len, char *title)
{
	dump_mem(addr, len, eeprom_read_byte, title);
}

void dump_ram(const uint8_t *addr, unsigned int len, char *title)
{
	dump_mem(addr, len, ram_read_byte, title);
}


#if 0
void dump_heap(void)
{
	extern unsigned int __brkval;

	dump_ram((uint8_t *) __malloc_heap_start,
		__brkval - (unsigned int) __malloc_heap_start,
		"=== Heap:");
}
#endif


/*
 * Memory Display
 *	md addr {len}
 */
command_ret_t do_dump_mem(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
//	static const uint8_t *addr;
//	static uint16_t length = 128;
	uint8_t (*readhow)(const uint8_t *);
		
	(void) cmdtp; (void) flag;

	if (argc < 2)
		return CMD_RET_USAGE;

	const uint8_t *addr;
	uint16_t length = 128;
	
	if (strchr(argv[0],'r') != NULL)
		readhow = ram_read_byte;
	else if (strchr(argv[0],'e') != NULL)
		readhow = eeprom_read_byte;
	else
		return CMD_RET_USAGE;

	/* Address is specified since argc > 1 */
	addr = (const uint8_t *) (size_t) strtoul(argv[1], NULL, 16);

	/* If another parameter, it is the length to display. */
	if (argc > 2)
		length = (uint16_t) strtoul(argv[2], NULL, 16);

	/* Print the lines. */
	dump_mem(addr, length, readhow, NULL);

	return CMD_RET_SUCCESS;
}

command_ret_t do_eep_cp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
{
	uint16_t src, dest, count;
	int_fast8_t step;

	(void) cmdtp;
	(void) flag;

	if (argc != 4)
		return CMD_RET_USAGE;

	src = (size_t) strtoul(argv[1], NULL, 16);
	dest = (size_t) strtoul(argv[2], NULL, 16);
	count = (size_t) strtoul(argv[3], NULL, 16);

	if (src > E2END) {
		debug("src > EEPROM size: 0x%04x\n", src);
		return CMD_RET_FAILURE;
	}
	if (dest > E2END) {
		debug("dest > EEPROM size: 0x%04x\n", dest);
		return CMD_RET_FAILURE;
	}
	if (count > E2END+1) {
		debug("count > EEPROM size: 0x%04x\n", count);
		return CMD_RET_FAILURE;
	}
	if (count == 0) {
		debug("Zero length?\n");
		return CMD_RET_FAILURE;
	}

	if (dest > src) {
		src += count - 1;
		dest += count - 1;
		step = -1;
	} else
		step = 1;

	while (count-- > 0) {
		uint8_t data;
		data = eeprom_read_byte((uint8_t *) src);
		eeprom_write_byte((uint8_t *) dest, data);
		src += step;
		dest += step;

	}
	return CMD_RET_SUCCESS;
}

/*------------------------------------------------------------------------------*/


#if 0

struct __freelist {
	size_t sz;
	struct __freelist *nx;
};

extern char *__brkval;		/* first location not yet allocated */
extern struct __freelist *__flp; /* freelist pointer (head of freelist) */

#define STACK_POINTER() ((char *)AVR_STACK_POINTER_REG)

void
printfreelist(const char * title)
{
	struct __freelist *fp1;
	int i;
	unsigned int freesum = 0;

	if (!__flp) {
		printf("%s no free list\n", title ? title : "");
	} else {
		printf("Free list: %s\n", title ? title : "");
		for (i = 0, fp1 = __flp; fp1; i++, fp1 = fp1->nx) {
			printf("    entry %d @ %04x: size %4u, next ",
			       i, (size_t)fp1, fp1->sz);
			if (fp1->nx)
				printf("%04x\n", (size_t)fp1->nx);
			else
				printf("NULL\n");
			freesum += fp1->sz;
		}
	}
	
	freesum +=  (size_t) STACK_POINTER() - __malloc_margin - (size_t) __brkval;

	printf("SP: %04x, __brkval: %04x, Total free: %04u\n",
		(size_t) STACK_POINTER(), (size_t) __brkval, freesum);
}

#endif

#endif /* DEBUG */