/* diversity-nmea.c - DiversityNmea
 *
 * Copyright 2008 OpenMoko, Inc.
 * Authored by Chia-I Wu <olv@openmoko.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.
 */

#include <glib.h>
#include <diversity-nmea.h>
#include <diversity-equipment.h>
#include <string.h>
#include <stdlib.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>

#include "nmea-marshal.h"

enum {
	THROTTLE,
	GPRMC,
	LAST_SIGNAL
};

enum {
	PROP_0,
	PROP_DEVICE,
	PROP_LOG,
};

#define DIVERSITY_NMEA_GET_PRIVATE(o) (G_TYPE_INSTANCE_GET_PRIVATE((o), DIVERSITY_TYPE_NMEA, DiversityNmeaPrivate))
typedef struct _DiversityNmeaPrivate DiversityNmeaPrivate;

struct _DiversityNmeaPrivate {
	gchar *device_path;
	gint speed;

	GIOChannel *ch;
	guint source_id;

	gint throttle;
	gboolean hard_throttle;

	GSList *pipes;

	gchar *log;
	GIOChannel *log_ch;
};

G_DEFINE_TYPE(DiversityNmea, diversity_nmea, DIVERSITY_TYPE_EQUIPMENT);

static guint nmea_signals[LAST_SIGNAL] = { 0 };

static gboolean diversity_nmea_parse_gprmc(DiversityNmea *nmea, gchar **fields, gint num_fields);

const static struct {
	const gchar *type;
	gint num_fields;
	gboolean (*parse)(DiversityNmea *nmea, gchar **fields, gint num_fields);
} nmea_types[] = {
	{ "GPRMC", 13, diversity_nmea_parse_gprmc },
};
static gint num_nmea_types = G_N_ELEMENTS(nmea_types);
#define MAX_NUM_FIELDS 13

static gdouble nmea_degree(gchar *field, gint hh)
{
	gdouble hour, min;
	gchar c;

	if (strlen(field) < hh)
		return 0.0;

	c = field[hh];
	field[hh] = '\0';

	hour = g_ascii_strtod(field, NULL);

	field[hh] = c;

	min = g_ascii_strtod(field + hh, NULL);

	return hour + min / 60.0;
}

static gboolean diversity_nmea_parse_gprmc(DiversityNmea *nmea, gchar **fields, gint num_fields)
{
	gdouble lon, lat;
	gboolean fix;

	if (num_fields < 7)
		return FALSE;

	fix = (*(fields[2]) == 'A');
	if (!fix) {
		g_signal_emit(nmea, nmea_signals[GPRMC], 0,
				fix, 0.0, 0.0);

		return TRUE;
	}

	lat = nmea_degree(fields[3], 2);
	if (*(fields[4]) == 'S')
		lat = -lat;

	lon = nmea_degree(fields[5], 3);
	if (*(fields[6]) == 'W')
		lon = -lon;

	g_signal_emit(nmea, nmea_signals[GPRMC], 0, fix, lon, lat);

	return TRUE;
}

inline static gint nmea_split(gchar *sentence, gchar **fields, gint num_fields)
{
	gchar *p;
	gint n = 0;

	if (!sentence)
		return 0;

	fields[n++] = sentence;

	for (p = sentence; *p; p++) {
		if (*p == ',') {
			*p = '\0';
			fields[n++] = p + 1;

			if (n == num_fields) {
				p++;

				p = strchr(p, ',');
				if (p)
					*p = '\0';

				break;
			}
		}
	}

	return n;
}

inline static gboolean nmea_validate(const char *sentence)
{
	const gchar *p = sentence;
	gint calculated = 0, sum;

	p++;

	while (*p && *p != '*')
		calculated ^= *p++;

	if (*p++ != '*')
		return FALSE;

	sum = strtol(p, NULL, 16);

	return (calculated == sum);
}

static gboolean diversity_nmea_parse(DiversityNmea *nmea, gchar *sentence)
{
	gchar *fields[MAX_NUM_FIELDS];
	gint i, n;

	if (*sentence != '$')
		return FALSE;

	for (i = 0; i < num_nmea_types; i++) {
		if (strncmp(sentence + 1, nmea_types[i].type,
					strlen(nmea_types[i].type)) == 0)
			break;
	}

	if (i == num_nmea_types)
		return FALSE;

	if (!nmea_validate(sentence))
		return FALSE;

	n = nmea_split(sentence, fields, nmea_types[i].num_fields);

	return nmea_types[i].parse(nmea, fields, n);
}

inline static void diversity_nmea_pipe(DiversityNmea *nmea, gchar *buf, gssize size)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);
	GSList *tmp_list;
	GError *error = NULL;

	tmp_list = priv->pipes;
	while (tmp_list) {
		GIOChannel *pipe = tmp_list->data;

		if (g_io_channel_write_chars(pipe, buf, size, NULL,
					&error) != G_IO_STATUS_NORMAL) {
			g_warning("failed to pipe to %p: %s\n",
					pipe, error->message);

			g_error_free(error);
		}

		tmp_list = tmp_list->next;
	}
}

static gboolean device_io(GIOChannel *channel, GIOCondition condition, gpointer data)
{
	DiversityNmea *nmea = DIVERSITY_NMEA(data);
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);
	gchar *line;
	gsize len;
	GIOStatus status;
	gchar *message;

	switch (condition) {
	case G_IO_IN:
	case G_IO_PRI:
		/* FIXME this is heavy */
		status = g_io_channel_read_line(priv->ch, &line, &len, NULL, NULL);
		if (status == G_IO_STATUS_AGAIN) {
			break;
		} else if (status != G_IO_STATUS_NORMAL) {
			message = "failed to read";

			goto fail;
		}

		diversity_nmea_pipe(nmea, line, len);
		diversity_nmea_parse(nmea, line);
		g_free(line);
		break;
	case G_IO_ERR:
	case G_IO_HUP:
		message = "connection broken";

		goto fail;
		break;
	default:
		message = "unknown IO condition";

		goto fail;
		break;
	}

	return TRUE;

fail:
	g_warning("device_io failed(%d): %s", condition, message);
	priv->source_id = 0;

	return FALSE;
}

inline static speed_t to_baud_rate(gint speed)
{
	speed_t val;

	switch (speed) {
	case 0:
		val = B0;
		break;
	case 50:
		val = B50;
		break;
	case 75:
		val = B75;
		break;
	case 110:
		val = B110;
		break;
	case 134:
		val = B134;
		break;
	case 150:
		val = B150;
		break;
	case 200:
		val = B200;
		break;
	case 300:
		val = B300;
		break;
	case 600:
		val = B600;
		break;
	case 1200:
		val = B1200;
		break;
	case 1800:
		val = B1800;
		break;
	case 2400:
		val = B2400;
		break;
	case 4800:
		val = B4800;
		break;
	case 9600:
		val = B9600;
		break;
	case 19200:
		val = B19200;
		break;
	case 38400:
		val = B38400;
		break;
	case 57600:
		val = B57600;
		break;
	case 115200:
		val = B115200;
		break;
	case 230400:
		val = B230400;
		break;
	default:
		val = B0;
		break;
	}

	return val;
}

static int serial_open(const gchar *path, speed_t speed)
{
	int fd;
	struct termios term;

	fd = open(path, O_RDONLY | O_NOCTTY | O_NONBLOCK);
	if (fd < 0)
		return fd;

	if (speed == B0)
		return fd;

	if (tcgetattr(fd, &term) < 0) {
		g_warning("tcgetattr failed");
		close(fd);

		return -1;
	}

	term.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
	term.c_cflag |= CREAD | CLOCAL;

	term.c_iflag &= ~(PARMRK | INPCK);
	term.c_cflag &= ~(CSIZE | CSTOPB | PARENB | PARODD);
	term.c_cflag |= CS8;

	term.c_lflag &= ~ICANON;
	term.c_lflag &= ~ECHO;

	cfsetspeed(&term, speed);

	if (tcsetattr(fd, TCSANOW, &term) < 0) {
		g_warning("tcsetattr failed");
		close(fd);

		return -1;
	}

	return fd;
}

static void diversity_nmea_set_ds(DiversityNmea *nmea, const gchar *ds)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);
	gchar *p;
	int fd;

	if (priv->source_id) {
		g_source_remove(priv->source_id);
		priv->source_id = 0;
	}

	if (priv->device_path) {
		g_free(priv->device_path);
		g_io_channel_unref(priv->ch);
	}

	if (!ds) {
		if (priv->device_path) {
			priv->device_path = NULL;
			priv->ch = NULL;
			g_object_notify(G_OBJECT(nmea), "device");
		}

		return;
	}

	priv->device_path = g_strdup(ds);
	priv->speed = 0;

	/* blah */
	p = strrchr(priv->device_path, ':');
	if (p) {
		*p++ = '\0';
		priv->speed = atoi(p);
	}

	fd = serial_open(priv->device_path, to_baud_rate(priv->speed));
	if (fd < 0) {
		g_warning("failed to open %s", priv->device_path);

		g_free(priv->device_path);
		priv->device_path = NULL;
		priv->ch = NULL;
		g_object_notify(G_OBJECT(nmea), "device");

		return;
	}

	priv->ch = g_io_channel_unix_new(fd);
	g_io_channel_set_encoding(priv->ch, NULL, NULL);
	g_io_channel_set_close_on_unref(priv->ch, TRUE);

	g_io_add_watch(priv->ch, G_IO_IN | G_IO_PRI | G_IO_ERR | G_IO_HUP,
			device_io, nmea);

	g_object_notify(G_OBJECT(nmea), "device");
}

static gboolean diversity_nmea_set_config(DiversityEquipment *eqp, const gchar *key, const GValue *val)
{
	DiversityNmea *nmea = DIVERSITY_NMEA(eqp);
	gboolean success = FALSE;

	if (strcmp(key, "device-path") == 0 &&
	    G_VALUE_TYPE(val) == G_TYPE_STRING) {
		diversity_nmea_set_ds(nmea, g_value_get_string(val));

		success = TRUE;
	} else if (strcmp(key, "log") == 0 &&
		   G_VALUE_TYPE(val) == G_TYPE_STRING) {
		diversity_nmea_set_log(nmea, g_value_get_string(val));

		success = TRUE;
	}

	return success;
}

static gboolean diversity_nmea_get_config(DiversityEquipment *eqp, const gchar *key, GValue *val)
{
	DiversityNmea *nmea = DIVERSITY_NMEA(eqp);
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);
	gboolean success = FALSE;

	// XXX should I?
	//g_value_init(val, G_TYPE_STRING);

	if (strcmp(key, "device-path") == 0 &&
	    G_VALUE_TYPE(val) == G_TYPE_STRING) {
		g_value_set_string(val, priv->device_path);

		success = TRUE;
	} else if (strcmp(key, "log") == 0 &&
		   G_VALUE_TYPE(val) == G_TYPE_STRING) {
		g_value_set_string(val, priv->log);

		success = TRUE;
	}

	return success;
}

static gboolean diversity_nmea_equip(DiversityEquipment *eqp)
{
	return TRUE;
}

static void diversity_nmea_unequip(DiversityEquipment *eqp)
{
}

static void diversity_nmea_dispose(GObject *obj)
{
	((GObjectClass *) diversity_nmea_parent_class)->dispose(obj);
}

static void diversity_nmea_finalize(GObject *obj)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(obj);
	GSList *tmp_list;

	if (priv->source_id) {
		g_source_remove(priv->source_id);
		priv->source_id = 0;
	}

	if (priv->device_path) {
		g_free(priv->device_path);
		g_io_channel_unref(priv->ch);
	}

	g_free(priv->log);

	tmp_list = priv->pipes;
	while (tmp_list) {
		GIOChannel *ch = tmp_list->data;

		g_io_channel_unref(ch);

		tmp_list = tmp_list->next;
	}
	g_slist_free(priv->pipes);
	
	((GObjectClass *) diversity_nmea_parent_class)->finalize(obj);
}

static void diversity_nmea_set_property(GObject *obj, guint prop_id, const GValue *value, GParamSpec *pspec)
{
	DiversityNmea *nmea = DIVERSITY_NMEA(obj);

	switch (prop_id) {
	case PROP_DEVICE:
		diversity_nmea_set_ds(nmea, g_value_get_string(value));
		break;
	case PROP_LOG:
		diversity_nmea_set_log(nmea, g_value_get_string(value));
		break;
	default:
		G_OBJECT_WARN_INVALID_PROPERTY_ID(obj, prop_id, pspec);
		break;
	}
}

static void diversity_nmea_get_property(GObject *obj, guint prop_id, GValue *value, GParamSpec *pspec)
{
	DiversityNmea *nmea = DIVERSITY_NMEA(obj);
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);

	switch (prop_id) {
	case PROP_DEVICE:
		g_value_set_string(value, priv->device_path);
		break;
	case PROP_LOG:
		g_value_set_string(value, priv->log);
		break;
	default:
		G_OBJECT_WARN_INVALID_PROPERTY_ID(obj, prop_id, pspec);
		break;
	}
}

static void diversity_nmea_class_init(DiversityNmeaClass *klass)
{
	GObjectClass *o_class = (GObjectClass *) klass;
	DiversityEquipmentClass *eqp_class = (DiversityEquipmentClass *) klass;

	o_class->set_property = diversity_nmea_set_property;
	o_class->get_property = diversity_nmea_get_property;
	o_class->dispose = diversity_nmea_dispose;
	o_class->finalize = diversity_nmea_finalize;

	eqp_class->equip = diversity_nmea_equip;
	eqp_class->unequip = diversity_nmea_unequip;

	eqp_class->set_config = diversity_nmea_set_config;
	eqp_class->get_config = diversity_nmea_get_config;

	g_object_class_install_property(o_class,
					PROP_DEVICE,
					g_param_spec_string("device",
							    "Device",
							    "Device",
							    NULL,
							    G_PARAM_READABLE));

	g_object_class_install_property(o_class,
					PROP_LOG,
					g_param_spec_string("log",
							    "Log",
							    "Log",
							    NULL,
							    G_PARAM_READWRITE));

	nmea_signals[THROTTLE] =
		g_signal_new("throttle-changed",
			     G_TYPE_FROM_CLASS(klass),
			     G_SIGNAL_RUN_LAST,
			     G_STRUCT_OFFSET(DiversityNmeaClass, throttle_changed),
			     NULL, NULL,
			     _nmea_marshal_VOID__INT_BOOLEAN,
			     G_TYPE_NONE, 2,
			     G_TYPE_INT,
			     G_TYPE_BOOLEAN);

	nmea_signals[GPRMC] =
		g_signal_new("gprmc",
			     G_TYPE_FROM_CLASS(klass),
			     G_SIGNAL_RUN_LAST,
			     G_STRUCT_OFFSET(DiversityNmeaClass, gprmc),
			     NULL, NULL,
			     _nmea_marshal_VOID__BOOLEAN_DOUBLE_DOUBLE,
			     G_TYPE_NONE, 3,
			     G_TYPE_BOOLEAN,
			     G_TYPE_DOUBLE,
			     G_TYPE_DOUBLE);

	g_type_class_add_private(klass, sizeof(DiversityNmeaPrivate));
}

static void diversity_nmea_init(DiversityNmea *nmea)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);

	priv->device_path = NULL;
	priv->speed = 0;
	priv->ch = NULL;

	priv->throttle = 0;
	priv->hard_throttle = FALSE;

	priv->pipes = NULL;

	priv->log = NULL;
	priv->log_ch = NULL;
}

DiversityNmea *diversity_nmea_new(void)
{
	DiversityNmea *nmea;
	
	nmea = g_object_new(DIVERSITY_TYPE_NMEA,
			     "name", "nmea",
			     NULL);

	return nmea;
}

void diversity_nmea_set_log(DiversityNmea *nmea, const gchar *log)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);
	GError *error = NULL;

	if (priv->log) {
		diversity_nmea_remove_pipe(nmea, priv->log_ch);

		g_free(priv->log);
		priv->log = NULL;
	}

	priv->log_ch = g_io_channel_new_file(log, "w", &error);
	if (!priv->log_ch) {
		g_warning("failed to open %s: %s", log, error->message);
		g_error_free(error);
		g_object_notify(G_OBJECT(nmea), "log");

		return;
	}

	g_io_channel_set_encoding(priv->log_ch, NULL, NULL);
	g_io_channel_set_close_on_unref(priv->log_ch, TRUE);

	diversity_nmea_add_pipe(nmea, priv->log_ch);
	g_io_channel_unref(priv->log_ch);

	priv->log = g_strdup(log);

	g_object_notify(G_OBJECT(nmea), "log");
}

const gchar *diversity_nmea_get_log(DiversityNmea *nmea)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);

	return priv->log;
}

void diversity_nmea_throttle(DiversityNmea *nmea, gint sec, gboolean hard)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);

	if (priv->throttle != sec || priv->hard_throttle != hard) {
		priv->throttle = sec;
		priv->hard_throttle = hard;

		g_signal_emit(nmea, THROTTLE, 0, sec, hard);
	}
}

void diversity_nmea_add_pipe(DiversityNmea *nmea, GIOChannel *ch)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);

	g_io_channel_ref(ch);
	priv->pipes = g_slist_prepend(priv->pipes, ch);
}

void diversity_nmea_remove_pipe(DiversityNmea *nmea, GIOChannel *ch)
{
	DiversityNmeaPrivate *priv = DIVERSITY_NMEA_GET_PRIVATE(nmea);
	GSList *tmp_list;

	tmp_list = g_slist_find(priv->pipes, ch);
	if (tmp_list) {
		g_io_channel_unref(ch);
		priv->pipes = g_slist_remove(priv->pipes, ch);
	}
}