#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"
#include "devices.h"
#include "memory.h"

#define VERSION "Swansea Hackspace PIC Bootloader v1"

#define BAUD_RATE 19200

#ifdef _WIN32
#define PORT_NAME "COM4:"
#else 
#define PORT_NAME "/dev/ttyUSB0"
#endif

void usage(const char * name)
{
	loge(VERSION);
	loge("Usage: %s [-b baud] [-p port] [-i] [-v] {file.hex}", name);
	loge("-b baud	Set baudrate (default: %d)", BAUD_RATE);
	loge("-p port	Set serial port (default: %s)", PORT_NAME);
	loge("-i	ID Only mode.");
	loge("-v	Verify after write");
}


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

int bitmask(int value)
{
	int out = 0;
	if (value >= (1<<1)) out |= 1;
	if (value >= (1<<2)) out |= 1<<1;
	if (value >= (1<<3)) out |= 1<<2;
	if (value >= (1<<4)) out |= 1<<3;
	if (value >= (1<<5)) out |= 1<<4;
	if (value >= (1<<6)) out |= 1<<5;
	if (value >= (1<<7)) out |= 1<<6;
	if (value >= (1<<8)) out |= 1<<7;
	if (value >= (1<<9)) out |= 1<<8;
	if (value >= (1<<10)) out |= 1<<9;

	return out;
}
/*
 * This routine writes a memory plan out to the device
 */
int update_mem(port_t * pt, const devid_t * dev, uint16_t maxmem, mem_t * ram, int verify)
{
	mem_t * block = ram;
	uint8_t buff[1024];  // scratch space

	for (block=ram; block != NULL; block=block->next) {
		int bstart = block->start / 2;

		/* skip config words, we cant write them anyway */
		if (bstart >= 0x8000) {
			logd("UpdateMem: skip config block @ %04x", bstart);
			continue;
		}
			

		uint8_t * p = block->bytes;
		int left = block->len;
		uint16_t addr = bstart;
		int rowlen = dev->rowsize * 2;
		logd("UpdateMem: new block %d bytes @ %04x", block->len, bstart);

		while (left > 0) {
			int off = 0;
			int len = rowlen;

			memset(buff, 255, sizeof(buff));
			if (addr == bstart) {
				/* first row, align the start */
				if (addr % dev->rowsize != 0) {
					addr &= ~bitmask(dev->rowsize);
					off = (bstart - addr) * 2;
					len -= off;
					logd("UpdateMem: realigning %04X to %04X", bstart, addr);
				}
			}

			len = MIN(len, left);

			logd("UpdateMem: Preparing %d bytes @ %04X", len, addr);

			/* partial row write, read first */
			if (off != 0 || len < rowlen) {
				logd("UpdateMem: Read %d words @ %04x", dev->rowsize, addr);
				if (loader_readmem(pt, addr, buff, dev->rowsize)) {
					loge("UpdateMem: Aborting on failed read");
					return 1;
				}
				print_memory(addr, buff, dev->rowsize);
			}

			/* update with new values */
			memcpy(&buff[off], p, len);

			/* write the row */
			logd("UpdateMem: Writing %d words @ %04x", dev->rowsize, addr);
			print_memory(addr, buff, dev->rowsize);
			if (loader_writemem(pt, addr, buff, dev->rowsize)) {
				loge("UpdateMem: Aborting on failed write");
				return 1;
			}

			if (verify) {
				uint8_t again[1024];
				logd("UpdateMem: Verify %d words @ %04x", dev->rowsize, addr);
				if (loader_readmem(pt, addr, again, dev->rowsize)) {
					loge("UpdateMem: Aborting on failed read");
					return 1;
				}
				print_memory(addr, again, dev->rowsize);
				for (int i=0; i<dev->rowsize; i++) {
					if (again[i] != buff[i]) {
						loge("UpdateMem: Verify failed on block 0x%04X", addr);
						return 1;
					}
				}
			}

			/* shuffle along */
			left -= len;
			p += len;
			addr += dev->rowsize;
		}
	}
	return 0;
}

int main(int argc, char **argv)
{
	int opt;
	char * port = NULL;
	int baud = BAUD_RATE;
	int verify = 0;
	int idonly = 0;

	while ((opt=getopt(argc, argv, "h?b:p:ivd"))!=-1) {
		switch (opt) {
			case 'b':
				baud = atoi(optarg);
				break;
			case 'p':
				if (port) free(port);
				port = strdup(optarg);
				break;
			case 'v':
				verify=1;
				break;
			case 'i':
				idonly=1;
				break;
			case 'd':
				debug++;
				break;
			case 'h':
			case '?':
			default:
				usage(argv[0]);
				return 1;
		}
	}

	if (port == NULL) port = PORT_NAME;

	if (!idonly && optind >= argc) {
		loge("Error: missing hexfile");
		loge("");
		usage(argv[0]);
		return 1;
	}

	mem_t * ram = NULL;

	if (!idonly) {
		FILE * fd = NULL;
		if ((fd = fopen(argv[optind], "r"))==NULL) {
			loge("Error opening %s: %s", argv[optind], strerror(errno));
			return 1;
		}
		ram = load_ihex(fd);
		fclose(fd);

		logd("Memory Summary :-");
		list_mem(ram);
	}

	logd("open serial port %s at %d baud", port, baud);
	port_t * pt = serial_open(port, baud);
	if (pt == NULL) {
		return 1;
	}

	uint16_t maxmem;
	uint16_t devid;
	if (loader_connect(pt, &maxmem, &devid)) {
		return 1;
	}

	const devid_t * dev = devid_to_info(devid);

	if (idonly) {
		logi("Device ID: %04X", devid);
		logi(" Free Mem: %d words available", maxmem);
		if (dev != NULL) {
			logi(" Dev Name: %s", dev->name );
			logi("  Max Mem: %d", dev->memsize );
		}

		return 0;
	}
	if (dev) logd("Device Name: %s", dev->name);

	/* check that the selected program will fit on this device */
	if (makesafe_mem(&ram, maxmem)) {
		serial_close(pt);
		return 1;
	}
	logd("After re-organisation");
	list_mem(ram);

	/* now write the updated memory plan to the device */
	if (!update_mem(pt, dev, maxmem, ram, verify)) {
		if (verify)
			logi("Device Write (and Verify) Complete");
		else
			logi("Device Write Complete");
	}

	/* finished */
	serial_close(pt);
	logd("done.");
	return 0;

}