#include <fcntl.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <stdlib.h>	/* exit */
#include <termios.h>
#include <unistd.h>
#include <ctype.h>
#include <sys/time.h>	/* timeval */

#include <readline/readline.h>
#include <readline/history.h>

#define GPS_DEV "/dev/ttySAC1"

#define GPS_SYNC "\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80\x80"
#define GPS_INIT "\xff\x00\xfd\xc0\x00\xfc"

/* turn a gta01-pm-gps.0 pin on or off */
int sysfs_gps_pin(char *name, int value)
{
	static char device_path[512] = "/sys/bus/platform/devices/gta01-pm-gps.0/";
	strcpy(&device_path[41], name);
	int fd;
	fd = open(device_path, O_WRONLY);
	if (fd < 0)
		return fd;
	write(fd, value ? "1" : "0", 2);
	close(fd);
}

/* power on sequence for hammerhead on GTA01 */
void power_on_seq(void)
{
	// reset 0
	// pwron 0
	sysfs_gps_pin("power_lp_io_3v3", 1);
	sysfs_gps_pin("power_lp_io_3v3", 1);
	sysfs_gps_pin("reset", 1);
	usleep(1000);
	sysfs_gps_pin("power_pll_core_2v5", 1);
	sysfs_gps_pin("power_avdd_3v", 1);
	sysfs_gps_pin("power_vdd_core_1v5", 1); // power_core_1v5
	usleep(1000);
	sysfs_gps_pin("pwron", 1);
	usleep(15900);
}

/* power off sequence for hammerhead on GTA01 */
void power_off_seq(void)
{
	sysfs_gps_pin("power_pll_core_2v5", 0);
	sysfs_gps_pin("power_avdd_3v", 0);
	sysfs_gps_pin("power_vdd_core_1v5", 0); // power_core_1v5
	sysfs_gps_pin("pwron", 0);
}

/* return the byte stream in 8bit hex representation */
char *hexstr(unsigned char *data, int size)
{
	static char *hex = "0123456789abcdef";
	unsigned char *s = malloc(size*3);
	int i = 0;
	while (i < size) {
		*s++ = hex[data[i]>>4];
		*s++ = hex[data[i]&0xf];
		*s++ = ' ';
		i++;
	}
	*--s = '\0';
}

/* return the number of hex digits until
   the first occurence of a non-digit or
   end of string */
int digits(char *s)
{
	int i = 0;
	while isxdigit(*s++)
		i++;
	return i;
}

/* return the decimal value of a hex char */
int unhexchar(char c)
{
	if (c < '0')
		return -1;
	if (c <= '9')
		return c-'0';
	if (c < 'A')
		return -1;
	if (c <= 'F')
		return c-'A'+10;
	if (c < 'a')
		return -1;
	if (c <= 'f')
		return c-'a'+10;
	return -1;
}

/* convert a hex string into a byte stream.
   each word (separated by whitespace) is parsed as
   a little-endian number of 8,16,24 or 32bit */
char *unhexstr(char *s, int *size)
{
	int len = 0;
	char *ret;
	char *p = s;
	if (!size)
		return NULL;
	while (*p) {
		int d = digits(p);
		if (d == 0) {
			if (!isblank(*p++))
				return NULL;
		} else if (d <= 8) {
			len += (d+1)>>1;
			p += d;
		} else
			return NULL;
	}
	*size = len;
	p = ret = malloc(len);
	while (*s) {
		if (isblank(*s))
			s++;
		else {
			int d = digits(s);
			switch (d) {
			case 1:
				*p++ = unhexchar(s[0]);
				break;
			case 2:
				*p++ = unhexchar(s[0])<<4|unhexchar(s[1]);
				break;
			case 3:
				*p++ = unhexchar(s[1])<<4|unhexchar(s[2]);
				*p++ = unhexchar(s[0]);
				break;
			case 4:
				*p++ = unhexchar(s[2])<<4|unhexchar(s[3]);
				*p++ = unhexchar(s[0])<<4|unhexchar(s[1]);
				break;
			case 5:
				*p++ = unhexchar(s[3])<<4|unhexchar(s[4]);
				*p++ = unhexchar(s[1])<<4|unhexchar(s[2]);
				*p++ = unhexchar(s[0]);
				break;
			case 6:
				*p++ = unhexchar(s[4])<<4|unhexchar(s[5]);
				*p++ = unhexchar(s[2])<<4|unhexchar(s[3]);
				*p++ = unhexchar(s[0])<<4|unhexchar(s[1]);
				break;
			case 7:
				*p++ = unhexchar(s[5])<<4|unhexchar(s[6]);
				*p++ = unhexchar(s[3])<<4|unhexchar(s[4]);
				*p++ = unhexchar(s[1])<<4|unhexchar(s[2]);
				*p++ = unhexchar(s[0]);
			case 8:
				*p++ = unhexchar(s[6])<<4|unhexchar(s[7]);
				*p++ = unhexchar(s[4])<<4|unhexchar(s[5]);
				*p++ = unhexchar(s[2])<<4|unhexchar(s[3]);
				*p++ = unhexchar(s[0])<<4|unhexchar(s[1]);
			}
			s += d;
		}
	}
	return ret;
}

/* change the output of decode.py's SEND lines into a packet
   (string of hex digits that can be consumed by unhexstr) */
char *parse_sendcmd(char *cmd)
{
	char *p;
	if (!strncmp(cmd,"SEND",4)) {
		cmd[0] = 'f';
		cmd[1] = 'e';
	//} else if (!strncmp(cmd, "CMD ",4)) {
	//	cmd[0] = 'f';
	//	cmd[1] = 'f';
	} else {
		printf("only SEND supported\n");
		return NULL;
	}
	cmd[2] = ' ';
	cmd[3] = ' ';
	if (!((cmd[5] == 'L') && isxdigit(cmd[7]) && isxdigit(cmd[8]))) {
		printf("L parameter missing\n");
		return NULL;
	}
	cmd[5] = ' ';
	if (!((cmd[10] == 'C' && cmd[11] == 'H') && isxdigit(cmd[13]))) {
		printf("CH parameter missing\n");
		return NULL;
	}
	cmd[10] = 'f';
	cmd[11] = 'd';
	if (!((cmd[15] == 'F' && cmd[16] == 'L') && isxdigit(cmd[18]))) {
		printf("FL parameter missing\n");
		return NULL;
	}
	cmd[15] = cmd[18];
	cmd[16] = cmd[13];
	cmd[13] = ' ';
	cmd[18] = ' ';
	if (!((cmd[20] == 'C' && cmd[21] == 'M' && cmd[22] == 'D') && isxdigit(cmd[24]) && isxdigit(cmd[25]))) {
		printf("CMD parameter missing\n");
		return NULL;
	}
	cmd[20] = cmd[24];
	cmd[21] = cmd[25];
	cmd[22] = ' ';
	if (cmd[26] != ':') {
		printf("':' missing\n");
		return NULL;
	}
	p = cmd+23;
	while (p[4]) {
		*p++ = *(p+4);
	}
	*p++ = ' ';
	*p++ = 'f';
	*p++ = 'c';
	*p = 0;
	return cmd;
}

/* rudimentary packet validation */
int check_packet(char *p, int len)
{
	int L, end;
	if (len < 6) {
		printf("too small\n");
		return 0;
	}
	if ((p[0]|1) != 0xff) {
		printf("must start with fe or ff\n");
		return 0;
	}
	if (p[2] != 0xfd) {
		printf("third byte must be fd\n");
		return 0;
	}
	L = p[1];
	if (p[0] & 1)
		end = 5;
	else
		end = 5+4+(L<<2);
	if (end >= len) {
		printf("packet with L=%d must be %d bytes", L, end+1);
		return 0;
	}
	if  (p[end] != 0xfc) {
		printf("must end with fc (at %d)\n", end);
		return 0;
	}
	return 1;
}

/* send a packet over the uart and dump it to stdout */
void send(int fd, char *data, int size)
{
	char *s = hexstr(data, size);
	int n;
	printf("SEND(%d) %s\n", size, s);
	free(s);
	n = write(fd, data, size);
}

int recv_packet(int fd, char *data, int size)
{
	int n;
	fd_set readset;
	struct timeval tv;
	FD_ZERO(&readset);
	FD_SET(fd, &readset);
	tv.tv_sec = 0;
	tv.tv_usec = 100000;
	select(fd+1, &readset, NULL, NULL, &tv);
	n = read(fd, data, size);
	if (n < 0)
		return n;
	while (n < 6) {
		select(fd+1, &readset, NULL, NULL, NULL);
		n += read(fd, data+n, size-n);
	}
	while (n < (10+data[1]*4)) {
		select(fd+1, &readset, NULL, NULL, NULL);
		n += read(fd, data+n, size-n);
	}
	return n;
}

int main(int argc, char *argv[])
{
	int fd;
	struct termios t;
	fd_set readset;
	unsigned char buf[512];
	unsigned char *p;
	int n;

	FD_ZERO(&readset);

	printf("power on\n");
	power_on_seq();

	fd = open(GPS_DEV, O_RDWR | O_NONBLOCK);
	if (fd < 0) {
		printf("Couldn't open %s\n", GPS_DEV);
		exit(1);
	}

	FD_SET(fd, &readset);

	ioctl(fd, TCGETS, &t);
	cfsetispeed(&t, B115200);
	cfsetospeed(&t, B115200);
	cfmakeraw(&t);
	ioctl(fd, TCSETS, &t);

	send(fd, GPS_SYNC, 16);
	send(fd, GPS_INIT, 6);

	n = select(fd+1, &readset, NULL, NULL, NULL);
	n = read(fd, buf, sizeof(buf));
	printf("RECV(%d) ", n);
	p = buf;
	while (n-- > 0)
		printf("%02x ", *p++);
	printf("\n");

	while (1) {
		char *packet;
		char *cmd = readline("> ");
		if (!cmd)
			break;
		add_history(cmd);
		if (!strncmp(cmd, "SEND ", 5)) {
			packet = parse_sendcmd(cmd);
			//printf("'%s'\n", packet);
			packet = unhexstr(packet, &n);
		} else
			packet = unhexstr(cmd, &n);

		if (packet) {
			if (check_packet(packet, n)) {
				send(fd, packet, n);

				/* dump received packets */
				n = recv_packet(fd, buf, sizeof(buf));

				if (n >= 0) {
					printf("RECV(%d) ", n);
					p = buf;
					while (n-- > 0)
						printf("%02x ", *p++);
					printf("\n");
				} else
					printf("no reply\n");
			} else
				printf("invalid packet\n");

			free(packet);
		} else
			printf("parse error\n");
		free(cmd);
	}

	printf("power off\n");
	power_off_seq();
}