#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <stdint.h>
#include <stdlib.h>

#include "log.h"
#include "memory.h"
#include "serial.h"
#include "devices.h"
#include "protocol.h"

#define MIN(a,b) ((a)<(b)?(a):(b))

/* return 0 for end of file. -1 for error */
int parse_ihex16(const char *line, uint8_t *bytes, int *addr, int *code)
{
	unsigned int sum, len, cksum;
	unsigned int laddr, lcode;
	int i;
	const char *p;

	if (line[0] != ':') return -1;
	if (strlen(line) < 11) return -1;
	p = &line[1];
	if (!sscanf(p, "%02x", &len)) return -1;
	p+=2;
	if (strlen(line) < (11 + (len * 2))) return -1;
	if (!sscanf(p, "%04x", &laddr)) return -1;
	p += 4;
	*addr = laddr; // little endian address record
	if (!sscanf(p, "%02x", &lcode)) return -1;
	p+=2;
	*code = lcode & 0xFF;

	/* end of file record */
	if (*code == 1) return 0;

	sum = (len & 0xFF) + ((*addr >> 8) & 0xFF) + (*addr & 0xFF) + (*code & 0xFF);

	i = 0;
	while (i < len) {
		unsigned int byte;
		/* files are little-endian */
		if (!sscanf(p, "%02x", &byte)) return -1;
		bytes[i+1] = byte & 0xFF;
		sum += byte & 0xFF;
		p += 2;

		if (!sscanf(p, "%02x", &byte)) return -1;
		bytes[i] = byte & 0xFF;
		sum += byte & 0xFF;
		i += 2;
		p += 2;
	}
	if (!sscanf(p, "%02x", &cksum)) return -1;
	if (  ((sum & 0xFF) + (cksum & 0xFF)) & 0xFF ) return -1;
	return len;
}

static void mem_add(mem_t **head, uint32_t start, uint8_t * bytes, int len)
{
	/* look for an existing block this overlaps/adjoins */
	mem_t * block = *head;
	uint32_t end = start + len;

	for(block = *head; block!=NULL; block=block->next) {
		int bend = block->start + block->len;
		/* before and not touching */
		if (start < block->start && end+1 < block->start) continue;
		/* after and not touching */
		if (start > bend+1) continue;

		/* therefore it at least touches */

		/* starts before or on old block */
		if (start <= block->start) {
			int pre=0, lap=0, post=0;
			pre = block->start - start; // bytes before the old block
			lap = len - pre;	// bytes overlapping, from start
			if (lap > block->len) {
				post = lap - block->len;
				lap = block->len;
			}
			int newsize = block->len + pre + post;
			uint8_t * newbytes = malloc(newsize);
			memcpy(&newbytes[pre], block->bytes, block->len);
			memcpy(newbytes, bytes, len);
			free(block->bytes);
			block->bytes = newbytes;
			block->len = newsize;
			block->start -= pre;
		} else {
			/* starts part way down / at end */
			int pre=0, /*lap=0, */ post=0;
			pre = start - block->start; // gap from start
			//lap = MIN(block->len - pre, len);
			if (end > bend) post = end - bend;
			int newsize = block->len + post;
			uint8_t * newbytes = malloc(newsize);
			memcpy(newbytes, block->bytes, block->len);
			memcpy(&newbytes[pre], bytes, len);
			block->bytes = newbytes;
			block->len = newsize;
		}
		return;
	}

	block = calloc(1, sizeof(mem_t));
	block->start = start;
	block->len = len;
	block->bytes = malloc(len);
	memcpy(block->bytes, bytes, len);
	block->next = *head;
	*head = block;
}

mem_t * load_ihex(FILE *in)
{
	uint32_t addrh = 0;

	char buff[1024];

	mem_t * ram = NULL;

	while (!feof(in) && fgets(buff, sizeof(buff), in)!=NULL) {
		if (buff[0] == '#') continue;

		unsigned char bytes[80];
		int len, addr, code;
		
		if ((len=parse_ihex16(buff, bytes, &addr, &code)) <= 0) {
			if (len < 0) loge("LoadIHEX: Bad line: %s\n", buff);
			continue;
		}

		if (code == 4) {
			addrh = ((bytes[0] << 8) | bytes[1]) << 16;
			logd("LoadIHEX: Setting high addr 0x%02X%02X", bytes[0], bytes[1]);
		} else
		if (code == 1) {
			/* end of file marker */
			break;
		} else
		if (code == 0) {
			/* normal code block */
			uint32_t fulladdr = addrh | addr;
			mem_add(&ram, fulladdr, bytes, len);
		}
	}
	return ram;
}

/* summary of the memory blocks in this list */
void list_mem(const mem_t * head)
{
	const mem_t *p = head;
	while (p!=NULL) {
		logd("%6X : %d bytes", p->start, p->len);
		p = p->next;
	}
}

/* check that the program will fit */
int validate_mem(mem_t * head, uint16_t maxmem)
{
	if (head == NULL) {
		loge("MemValidate: No program!");
		return 1;
	}

	mem_t * p;
	int used = 0;

	/* test that all the memory blocks will fit */
	for (p=head; p!=NULL; p=p->next) {
		/* just ignore config bytes, we cant write them anyway */
		if (p->start / 2 >= 0x8000) continue;

		used += p->len / 2;

		if (p->start / 2 >= maxmem || (p->start+p->len)/2 >= maxmem) {
			loge("MemValidate: Program too large, overlaps bootloader.");
			return 1;
		}
	}

	logd("MemValidate: Used %d of %d leaving %d free.", used, maxmem, maxmem-used);

	return 0;
}

/* reorganise bytes to work with the boot loader */
int makesafe_mem(mem_t **head, uint16_t maxmem)
{
	/* it must fit safely first */
	if (validate_mem(*head, maxmem)) return 1;

	mem_t * new = malloc(sizeof(mem_t));
	new->start = maxmem * 2;
	new->len = 8;
	new->bytes = malloc(8);
	memset(new->bytes, 255, 8);

	int found = 0;
	/* find the code that goes in words 0-3 and move it */
	mem_t * p = NULL;
	for (p = *head; p!=NULL; p=p->next) {
		if (p->start >= 0 && p->start <= 7) {
			int pre = p->start;
			int lap = MIN(8-pre, p->len);
			memcpy(&new->bytes[pre], p->bytes, lap);
			p->start += lap;
			p->len -= lap;
			memmove(p->bytes, &p->bytes[lap], p->len);
			found++;
		}
	}

	if (!found) {
		loge("MakeSafeMem: Could not find start vector");
		free(new);
		return 1;
	}

	/* check that there is something sane in here 
	 * Warning: this is very enhanced-midrange dependant */
	found = 0;
	uint8_t * m = new->bytes;
	uint16_t lath = 0;
	for (int i=0; i<4; i++) {
		uint16_t in = (m[0] << 8) | m[1];
		if ((in & 0x3F80) == 0x3180) { // MOVLP
			lath = in & 0x007F;
			logd("MakeSafeMem: 0x%04X MOVLP 0x%02X", i, lath);
		}else
		if ((in & 0x3800) == 0x2800) { // GOTO
			uint16_t addr = in & 0x07FF;
			addr |= lath << 8;
			logd("MakeSafeMem: 0x%04X GOTO 0x%02X", i, addr);

			if ( addr < 4) {
				// an absolute goto within the reset vector
				// this is silly and wrong, replace with NOP
				logd("MakeSafeMem: Rewriting bad 'GOTO 0x%02X' to NOP", addr);
				m[0] = 0;
				m[1] = 0;
			} else {
				// there is a sane GOTO, hurrah
				found = 1;
			}
		}
		m += 2;
	}

	if (!found) {
		loge("Start Vector did not contain a GOTO");
		free(new);
		return 1;
	}

	new->next = *head;
	*head = new;

	return 0;
}