/* $Id$ */
#include <sys/types.h>
#include <machine/endian.h>
#include <sys/limits.h>
#include <err.h>
#include <errno.h>
#include <fcntl.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>

static int ubx_send(int , char *, char *);
static unsigned short ubx_gen_checksum(char *, int);
char *gpsd_hexdump(char *, size_t);
int gpsd_hexpack(char *, char *, int);
int hex2bin(char *s);

int main(int argc, char **argv) {
	int speed, l, fd, n;
	struct termios term;
	char buf[BUFSIZ];
	time_t s, t;

	if (argc != 5){
		fprintf(stderr, "usage: nmeasend <speed> <port> ubx-type-hex ubx-body-hex\n");
		return 1;
	}

	if ((l = strlen(argv[4])) > 2*USHRT_MAX){
		fprintf(stderr, "oversized message\n");
		return 1;
	}

	if (l % 2){
		fprintf(stderr, "body must have an even number of hex digits\n");
		return 1;
	}

	speed = atoi(argv[1]);
	switch (speed) {
	case 230400:
	case 115200:
	case 57600:
	case 38400:
	case 28800:
	case 14400:
	case 9600:
	case 4800:
		break;
	default:
		fprintf(stderr, "invalid speed\n");
		return 1;
	}

	if ((fd = open(argv[2], O_RDWR | O_NONBLOCK | O_NOCTTY, 0644)) == -1)
		err(1, "open");

	tcgetattr(fd, &term);
	cfmakeraw(&term);
	cfsetospeed(&term, speed);
	cfsetispeed(&term, speed);
        term.c_cc[VMIN] = 1;
        term.c_cc[VTIME] = 0;
        term.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
        term.c_cflag |= CREAD | CLOCAL;
        term.c_iflag = term.c_oflag = term.c_lflag = (tcflag_t) 0;

	if (tcsetattr(fd, TCSANOW | TCSAFLUSH, &term) == -1)
		err(1, "tcsetattr");
	
	tcflush(fd, TCIOFLUSH);
	ubx_send(fd, argv[3], argv[4]);
	tcdrain(fd);
// return 0;
	t = 0; n = 0;
	while (1){
		usleep(10000);
		bzero(buf, BUFSIZ);
		if ((l = read(fd, buf, BUFSIZ)) == -1)
			if (!(EINTR == errno || EAGAIN == errno))
				err(1, "read");

		if (l > 0){
			printf("%s", gpsd_hexdump(buf, l));
			fflush(stdout);
		}
		if (((s = time(NULL)) > t) && (n < 3)){
			t = s;
			n++;
			ubx_send(fd, argv[3], argv[4]);
		}
	}
	return 0;
}

unsigned short ubx_gen_checksum(char *buf, int len){
	int n;
	unsigned short ck;
	unsigned char ck_a = 0, ck_b = 0;

	for (n = 0; n < len; n++) {
		ck_a += buf[n];
		ck_b += ck_a;
//		printf("n: %d c: 0x%02x ck_a: 0x%02x ck_b: 0x%02x\r\n", n, buf[n], ck_a, ck_b);
	}
	ck = ( (((unsigned short)ck_a & 0x00ff) << 8) | ((unsigned short)ck_b & 0x00ff));
#if BYTE_ORDER == LITTLE_ENDIAN
	ck = swap16(ck);
#endif
	return ck;

}

static int ubx_send(int fd, char *type, char *body ) {
	size_t status;
	char *buf;
	unsigned short l, n, ck;

	l = strlen(body) / 2;
	if ((buf = malloc(l+8)) == NULL)
		return -1;

	bzero(buf, l+8);
	buf[0] = 0xb5; buf[1] = 0x62;

	if (gpsd_hexpack(type, buf+2, 2) == -1){
		free(buf);
		return -1;
	}

#if BYTE_ORDER == BIG_ENDIAN
	n = swap16(l);
#else
	n = l;
#endif
	memcpy(buf+4, &n, 2);

	if (l)
		if (gpsd_hexpack(body, buf+6, l) == -1){
			free(buf);
			return -1;
		}

	ck = ubx_gen_checksum(buf+2, l+4);
	memcpy(buf+l+6, &ck, 2);

	fputs("\r\n>>>>>>>>>>>>", stderr);
	fputs(gpsd_hexdump(buf, l+8), stderr);
	fputs("\r\n>>>>>>>>>>>>\r\n", stderr);
	status = write(fd, buf, l+8);
	if (status == -1)
		perror("ubx_send");
	return (int)status;
}

char *gpsd_hexdump(char *binbuf, size_t binbuflen)
{
	static char hexbuf[USHRT_MAX*2+10+2];
	size_t i, j = 0;
	size_t len = (size_t)binbuflen;
	const char *ibuf = (const char *)binbuf;
	const char *hexchar = "0123456789abcdef";

	for (i = 0; i < len; i++) {
		if (((unsigned char)ibuf[i] == 0xb5) && ((unsigned char)ibuf[i+1] == 0x62)){
			hexbuf[j++] = '\n';
			hexbuf[j++] = '\n';
		}
		hexbuf[j++] = hexchar[ (ibuf[i]&0xf0)>>4 ];
		hexbuf[j++] = hexchar[ ibuf[i]&0x0f ];
	}
	hexbuf[j] ='\0';
	return hexbuf;
}

int gpsd_hexpack(char *src, char *dst, int len){
	int i, k, l;

	l = (int)(strlen(src) / 2);
	if ((l < 1) || (l > len))
		return -1;

	bzero(dst, len);
	for (i = 0; i < l; i++)
		if ((k = hex2bin(src+i*2)) != -1)
			dst[i] = (char)(k & 0xff);
		else
			return -1;
	return l;
}

int hex2bin(char *s)
{
	int a, b;

	a = s[0] & 0xff;
	b = s[1] & 0xff;

	if ((a >= 'a') && (a <= 'f'))
		a = a + 10 - 'a';
	else if ((a >= 'A') && (a <= 'F'))
		a = a + 10 - 'A';
	else if ((a >= '0') && (a <= '9'))
		a -= '0';
	else
		return -1;

	if ((b >= 'a') && (b <= 'f'))
		b = b + 10 - 'a';
	else if ((b >= 'A') && (b <= 'F'))
		b = b + 10 - 'A';
	else if ((b >= '0') && (b <= '9'))
		b -= '0';
	else
		return -1;

	return ((a<<4) + b);
}