/*
 * SkyTraq Venus 5 GPS API
 *
 * Copyright (C) 2010 by Multi-Tech Systems
 *
 * Author: James Maki <jmaki@multitech.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 */
#define _GNU_SOURCE

#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>
#include <stdarg.h>
#include <unistd.h>
#include <time.h>
#include <asm/byteorder.h>
#include <netdb.h>

#include "log.h"
#include "cbuffer.h"
#include "utils.h"
#include "venus_api.h"

static const struct baud_map __baud_map[] = {
	{B4800, 4800},
	{B9600, 9600},
	{B19200, 19200},
	{B38400, 38400},
	{B57600, 57600},
	{B115200, 115200},
};

static const struct baud_map __venus_baud_map[] = {
	{B4800, VENUS_4800},
	{B9600, VENUS_9600},
	{B19200, VENUS_19200},
	{B38400, VENUS_38400},
	{B57600, VENUS_57600},
	{B115200, VENUS_115200},
};

speed_t value_to_baud(speed_t value)
{
	int n = sizeof(__baud_map) / sizeof(struct baud_map);
	int i;

	for (i = 0; i < n; ++i) {
		if (__baud_map[i].value == value) {
			return __baud_map[i].baud;
		}
	}

	log_warning("baud rate not valid: %lu", (unsigned long) value);

	return (speed_t) - 1;
}

speed_t baud_to_venus(speed_t baud)
{
	int n = sizeof(__venus_baud_map) / sizeof(struct baud_map);
	int i;

	for (i = 0; i < n; ++i) {
		if (__venus_baud_map[i].baud == baud) {
			return __venus_baud_map[i].value;
		}
	}

	log_warning("baud rate not valid: %lu", (unsigned long) baud);

	return (speed_t) - 1;
}

static uint8_t seq_start[] = { 0xA0, 0xA1 };
static uint8_t seq_end[] = { 0x0D, 0x0A };

void venus_msg_data_free(struct venus_msg *msg)
{
	if (!msg) {
		return;
	}

	if (msg->data) {
		free(msg->data);
	}
	msg->data = NULL;
}

void venus_msg_free(struct venus_msg *msg)
{
	if (!msg) {
		return;
	}

	venus_msg_data_free(msg);
	free(msg);
}

uint8_t venus_checksum(void *data, uint16_t len)
{
	int i;
	uint8_t cs = 0;

	for (i = 0; i < len; i++) {
		cs ^= ((char *) data)[i];
	}

	return cs;
}

int venus_write_msg(int fd, struct venus_msg *msg)
{
	int err;
	uint8_t cs = 0;

	log_debug("id: %d", msg->data[0]);

	err = full_write(fd, seq_start, sizeof(seq_start));
	if (err != sizeof(seq_start)) {
		log_error("failed to write seq_start: %d %m", err);
	}
	msg->len = __cpu_to_be16(msg->len);
	err = full_write(fd, &msg->len, 2);
	if (err != 2) {
		log_error("failed to write len: %d %m", err);
	}
	msg->len = __be16_to_cpu(msg->len);
	err = full_write(fd, msg->data, msg->len);
	if (err != msg->len) {
		log_error("failed to write data: %d %m", err);
	}
	cs = venus_checksum(msg->data, msg->len);
	err = full_write(fd, &cs, 1);
	if (err != 1) {
		log_error("failed to write checksum: %d %m", err);
	}
	err = full_write(fd, seq_end, sizeof(seq_end));
	if (err != sizeof(seq_end)) {
		log_error("failed to write seq_end: %d %m", err);
	}

	return 0;
}

int venus_read_msg(int fd, struct venus_msg *msg)
{
	uint8_t cs;
	uint8_t *data;
	uint16_t len;
	uint8_t seq[2];
	int attempts = 0;

again:
	while (1) {
		safe_readn(fd, seq, 2);
		if (!memcmp(seq, seq_start, 2)) {
			break;
		}
		attempts++;
		if (attempts > 1024) {
			log_error("max attempts reached");
			return -1;
		}
	}

	safe_readn(fd, &len, 2);
	len = __be16_to_cpu(len);
	data = malloc(len);
	safe_readn(fd, data, len);
	safe_readn(fd, &cs, 1);
	if (venus_checksum(data, len) != cs) {
		log_error("checksum mismatch");
		return -1;
	}
	safe_readn(fd, seq, 2);
	if (memcmp(seq, seq_end, 2)) {
		log_error("expected seq end");
		return -1;
	}

	if (len == 2 && data[0] == ID_ACK && data[1] == ID_NONE) {
		log_debug("read ACK ID_NONE");
		free(data);
		goto again;
	}

	msg->data = data;
	msg->len = len;

#if DEBUG
	{
		int i;
		printf("READ MSG: ");
		for (i = 0; i < msg->len; i++) {
			printf("%02X ", msg->data[i]);
		}
		printf("\n");
	}
#endif

	return 0;
}

int venus_read_ack(int fd)
{
	int ret = -1;
	int err;

	struct venus_msg msg;

	err = venus_read_msg(fd, &msg);
	if (err < 0) {
		log_error("venus_read_msg failed");
		return ret;
	}

	if (msg.len == 2 && msg.data[0] == ID_ACK) {
		ret = 0;
	} else {
		log_error("msg is not ACK");
	}

	venus_msg_data_free(&msg);

	return ret;
}

int venus_write_msg_read_ack(int fd, struct venus_msg *msg)
{
	int err;

	err = venus_write_msg(fd, msg);
	if (err < 0) {
		log_error("venus_write_msg failed");
		return -1;
	}

	err = venus_read_ack(fd);
	if (err < 0) {
		log_error("venus_read_ack failed");
		return -1;
	}

	return 0;
}

ssize_t venus_read_nmea_sentence(int fd, void *buf, size_t count)
{
	ssize_t total = 0;
	char *cp = buf;
	int start = 0;
	char c;
	int err;

	if (!count) {
		return 0;
	}
	count--;

	while (count > 0) {
		err = safe_read(fd, &c, 1);
		if (err <= 0) {
			log_error("read from gps failed: %m");
			return -1;
		}

		if (!start) {
			if (c != '$') {
				continue;
			}
			start = 1;
		}

		cp[total++] = c;
		count--;

		if (c == '\n') {
			break;
		}
	}

	cp[total] = '\0';

	return total;
}

int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int16_t longitude, int16_t altitude)
{
	int err;
	uint8_t data[15];
	struct venus_msg msg;

	struct tm broken;
	struct tm *tmp;

	log_debug("begin");

	tmp = gmtime_r(&utc, &broken);
	if (tmp == NULL) {
		log_error("gmtime_r: %m");
		return -1;
	}

	data[0] = ID_SYSTEM_RESTART;
	data[1] = mode;

	uint16_t year = __cpu_to_le16(((uint16_t) broken.tm_year) + 1900);
	memcpy(data + 2, &year, 2);
	data[4] = (uint8_t) (broken.tm_mon + 1);
	data[5] = (uint8_t) broken.tm_mday;
	data[6] = (uint8_t) broken.tm_hour;
	data[7] = (uint8_t) broken.tm_min;
	data[8] = (uint8_t) broken.tm_sec;

	latitude = (int16_t) __cpu_to_le16((uint16_t) latitude);
	memcpy(data + 9, &latitude, 2);

	longitude = (int16_t) __cpu_to_le16((uint16_t) longitude);
	memcpy(data + 11, &longitude, 2);

	altitude = (int16_t) __cpu_to_le16((uint16_t) altitude);
	memcpy(data + 13, &altitude, 2);

	msg.data = data;
	msg.len = sizeof(data);
	err = venus_write_msg_read_ack(fd, &msg);
	if (err < 0) {
		log_error("venus_write_msg_read_ack failed");
		return -1;
	}

	log_debug("end");

	return 0;
}

int venus_factory_defaults(int fd, uint8_t type)
{
	int err;
	uint8_t data[2];
	struct venus_msg msg;

	data[0] = ID_FACTORY_DEFAULTS;
	data[1] = type;

	log_debug("begin");

	msg.data = data;
	msg.len = sizeof(data);
	err = venus_write_msg_read_ack(fd, &msg);
	if (err < 0) {
		log_error("venus_write_msg_read_ack failed");
		return -1;
	}

	log_debug("end");

	return 0;
}

int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr)
{
	int err;
	uint8_t data[4];
	struct venus_msg msg;

	data[0] = ID_CONF_SERIAL;
	data[1] = com;
	data[2] = baud;
	data[3] = attr;

	log_debug("begin");

	msg.data = data;
	msg.len = sizeof(data);
	err = venus_write_msg_read_ack(fd, &msg);
	if (err < 0) {
		log_error("venus_write_msg_read_ack failed");
		return -1;
	}

	log_debug("end");

	return 0;
}

int venus_conf_format(int fd, uint8_t type, uint8_t attr)
{
	int err;
	uint8_t data[3];
	struct venus_msg msg;

	data[0] = ID_CONF_FORMAT;
	data[1] = type;
	data[2] = attr;

	log_debug("begin");

	msg.data = data;
	msg.len = sizeof(data);
	err = venus_write_msg_read_ack(fd, &msg);
	if (err < 0) {
		log_error("venus_write_msg_read_ack failed");
		return -1;
	}

	log_debug("end");

	return 0;
}

int venus_query_sw_version(int fd, uint8_t type, struct venus_msg *msg)
{
	int err;
	uint8_t data[2];

	data[0] = ID_QUERY_SW_VERSION;
	data[1] = type;

	log_debug("begin");

	msg->data = data;
	msg->len = sizeof(data);
	err = venus_write_msg_read_ack(fd, msg);
	if (err < 0) {
		log_error("venus_write_msg_read_ack failed");
		return -1;
	}

	err = venus_read_msg(fd, msg);
	if (err < 0) {
		log_error("venus_read_msg: %d", err);
		return -1;
	}

	if (msg->len != 14 || msg->data[0] != ID_SW_VERSION) {
		venus_msg_data_free(msg);
		log_error("expected sw version message");
		return -1;
	}

	log_debug("end");

	return 0;
}

int venus_conf_nmea(int fd, uint8_t gga, uint8_t gsa, uint8_t gsv, uint8_t gll, uint8_t rmc, uint8_t vtg, uint8_t zda, uint8_t attr)
{
	int err;
	uint8_t data[9];
	struct venus_msg msg;

	data[0] = ID_CONF_NMEA;
	data[1] = gga;
	data[2] = gsa;
	data[3] = gsv;
	data[4] = gll;
	data[5] = rmc;
	data[6] = vtg;
	data[7] = zda;
	data[8] = attr;

	log_debug("begin");

	msg.data = data;
	msg.len = sizeof(data);
	err = venus_write_msg_read_ack(fd, &msg);
	if (err < 0) {
		log_error("venus_write_msg_read_ack failed");
		return -1;
	}

	log_debug("end");

	return 0;
}

int venus_tty_configure(int fd, speed_t baud_rate)
{
	struct termios tio;
	tcgetattr(fd, &tio);
	cfmakeraw(&tio);
	cfsetspeed(&tio, baud_rate);
	tcsetattr(fd, TCSANOW, &tio);

	return 0;
}

int venus_close(int fd)
{
	return close(fd);
}

int venus_open(const char *dev, speed_t baud_rate)
{
	int tty;

	tty = open(dev, O_RDWR | O_NOCTTY);
	if (tty < 0) {
		log_error("failed to open %s: %m", dev);
		exit(1);
	}

	venus_tty_configure(tty, baud_rate);
	tcflush(tty, TCIOFLUSH);

	return tty;
}