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

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

static uint8_t checksum(uint8_t * buff, int len)
{
	int sum = 0;
	int i;

	for (i=0;i<len;i++) sum += buff[i];
	return sum & 0xFF;
}

void print_memory(uint16_t addr, const unsigned char * buff, int words)
{
	int len = (words * 5) + 16;
	char * line = malloc(len);
	char * out = line;
	out += snprintf(out, len-strlen(line), "Mem %04X: ", addr);

	const unsigned char *p = &buff[0];
	for (int i=0; i<words; i++) {
		out += snprintf(out, len-strlen(line), "%02X%02X ", p[0], p[1] );
		p+=2;
	}
	logd(line);
	free(line);
}

void dumpbuff(const unsigned char * buff, int charlen)
{
	int len = (charlen * 3) + 16;
        char * line = malloc(len);
	char * out = line;
        out += snprintf(out, len-strlen(line), "Dump: ");

        const unsigned char *p = &buff[0];
        for (int i=0; i<charlen; i++) {
                out += snprintf(out, len-strlen(line), "%02X ", p[0] );
                p++;
        }
        logd(line);
        free(line);
}

int loader_readmem(port_t * pt, uint16_t addr, uint8_t * memory, int words)
{
	int len = (words * 2) + 4;
	unsigned char * buff = malloc(len);

	/* request to read memory block */
	buff[0] = 'R';
	buff[1] = addr >> 8;
	buff[2] = addr & 0xFF;
	buff[3] = checksum(buff, 3);

	int ret = serial_write(pt, buff, 4);
	if (ret <= 0) return 1;

	memset(buff, 0, len);

	/* read the response */
	if (serial_read(pt, buff, 1)<=0) return 1;

	if (buff[0] == 'E') {
		loge("ReadMem %04X: Loader gave error", addr);
		return 2;
	}

	if (buff[0] != 'R') {
		loge("ReadMem %04X: Unknown response to read 0x%02X '%c'", addr, buff[0], buff[0]);
		return 3;
	}

	/* now read the address, and check it matches */
	ret = serial_read(pt, &buff[1], len-1);
	if (ret < 1) {
		loge("ReadMem %04X: Error read response", addr);
		return 4;
	}
	if (ret < len-1) {
		loge("ReadMem %04X: Short read %d of %d", addr, ret, len-1);
		return 5;
	}

	int sum = checksum(buff, len-1);
	if ((sum & 0xFF) != buff[len-1]) {
		loge("ReadMem %04X: Bad checksum. %02X != %02X", addr, sum & 0xFF, buff[len-1]);
		return 6;
	}

	uint16_t realaddr = (buff[1]<<8) | buff[2];
	if (realaddr != addr) {
		loge("ReadMem %04X: Actual Address %04X", addr, realaddr);
		return 7;
	}
	logd("ReadMem %04X: Read Successful", realaddr);

	memcpy(memory, &buff[3], (words * 2));
	free(buff);

	return 0;
}


int loader_writemem(port_t * pt, uint16_t addr, uint8_t * memory, int words)
{
	int len = (words * 2) + 4;
	uint8_t * buff = malloc((words * 2) + 4);

	/* request to read memory block */
	buff[0] = 'W';
	buff[1] = addr >> 8;
	buff[2] = addr & 0xFF;

	unsigned char * p = memory;
	unsigned char * q = &buff[3];
	for (int i=0; i<words; i++) {
		*q++ = *p++;
		*q++ = *p++;
	}
	*q = checksum(buff, len-1) & 0xFF;

	logd("WriteMem %04X: Sending Write request", addr);
	int ret = serial_write(pt, buff, len);
	if (ret <= 0) { free(buff); return 1; }

	logd("WriteMem %04X: Awaiting Write confirmation", addr);
	/* read the response */
	if (serial_read(pt, buff, 1)<=0) { free(buff); return 1; }

	if (buff[0] == 'E') {
		loge("WriteMem %04X: Bootloader gave an error", addr);
		free(buff);
		return 2;
	}

	if (buff[0] != 'W') {
		loge("WriteMem %04X: unknown reponse '%c'", addr, buff[0]);
		free(buff);
		return 3;
	}

	/* now read the address, and check it matches */
	ret = serial_read(pt, &buff[1], 3);
	if (ret < 3) {
		loge("WriteMem %04X: Error reading rest of response %d of %d", addr, ret, 3);
		free(buff);
		return 4;
	}

	uint16_t realaddr = (buff[1]<<8) | buff[2];
	if (realaddr != addr) {
		loge("WriteMem %04X: Actual location %04X", addr, realaddr);
		free(buff);
		return 5;
	}
	int sum = checksum(buff, 3);

	if ((sum & 0xFF) != buff[3]) {
		loge("WriteMem %04X: Bad checksum on confirmation. %02X != %02X", addr, sum & 0xFF, buff[3]);
		dumpbuff(buff, 4);
		free(buff);
		return 8;
	}

	logd("WriteMem %04X: Write confirmed.", realaddr);
	free(buff);

	return 0;
}

/*
 * Connect to the bootloader
 * read the start of bootloader address
 */
int loader_connect(port_t * pt, uint16_t *maxmem, uint16_t *devid)
{
	if (pt == NULL) return 1;

	logd("Connect: Assert break and wait for acknowledgement");
	serial_break(pt, 1);

	unsigned char buff[10];
	int bootload = 0;
	int ret;

	logi("Please reset the device to enter bootloader mode");

	while ((ret=serial_read(pt, buff, 1))> 0) {
		if (buff[0] == 06) {
			break;
		} else {
			logd("Connect: Want ESC (0x06) got 0x%02X ret=%d", buff[0], ret);
			usleep(100000);
		}
	}

	if (ret <= 0) {
		if (ret == 0)
			loge("Connect: Serial port closed");
		else
			loge("Connect: read error: %s", strerror(errno));
		return 1;
	}

	/* Next we should see 'B' 'L' */
	while ((ret=serial_read(pt, &buff[1], 2))> 0) {
		if (buff[1] == 'B' && buff[2] == 'L') {
			bootload = 1;
			break;
		} else {
			logd("Connect: Want 'BL' got '%c%c' ret=%d", buff[1], buff[2], ret);
		}
		memset(buff,0,3);
	}

	if (ret <= 0) {
		if (ret == 0)
			loge("Connect: Serial port closed");
		else
			loge("Connect: read error: %s", strerror(errno));
		return 1;
	}

	/* turn break off now we are in bootloader mode */
	serial_break(pt, 0);

	if (!bootload) {
		serial_close(pt);
		loge("Connect: Could not find bootloader");
		return 1;
	}

	logd("Connect: Release break. Read memory size.");

	/* now read the bootloaders location, and thus mem size */
	ret=serial_read(pt, &buff[3], 5);

	if (ret < 3) {
		if (ret == 0)
			loge("Connect: Serial port closed");
		else
			loge("Connect: read error: %s", strerror(errno));
		return 1;
	}

	if (buff[7] != (checksum(buff,7) & 0xFF)) {
		loge("Connect: Bad checksum");
		return 1;
	}

	*devid = (buff[3] << 8) | buff[4];
	logd("Connect: Device ID: 0x%04X", *devid);

	*maxmem = (buff[5] << 8) | buff[6];
	logd("Connect: Memory Size: 0x%04X", *maxmem);

	return 0;
}