1 files changed, 424 insertions, 18 deletions

diff --git a/src/af-gps-binding.c b/src/af-gps-binding.c
index e78c7fd..c96ccf0 100644
--- a/src/af-gps-binding.c
+++ b/src/af-gps-binding.c
@@ -20,10 +20,12 @@
 #include <string.h>
 #include <unistd.h>
 #include <errno.h>
-#include <sys/types.h>
-#include <sys/socket.h>
 #include <netdb.h>
 #include <fcntl.h>
+#include <math.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
 
 #include <json-c/json.h>
 
@@ -31,8 +33,12 @@
 
 #include <afb/afb-binding.h>
 
-#define NAUTICAL_MILE_IN_METER     1852
-#define KNOT_TO_METER_PER_SECOND   0.5144444444444444
+#define NAUTICAL_MILE_IN_METER                     1852
+#define MILE_IN_METER                              1609.344
+#define KNOT_TO_METER_PER_SECOND                   0.5144444444         /* 1852 / 3600 */
+#define METER_PER_SECOND_TO_KNOT                   1.943844492          /* 3600 / 1852 */
+#define METER_PER_SECOND_TO_KILOMETER_PER_HOUR     3.6                  /* 3600 / 1000 */
+#define METER_PER_SECOND_TO_MILE_PER_HOUR          2.236936292          /* 3600 / 1609.344 */
 
 /*
  * references:
@@ -61,8 +67,42 @@ struct gps {
 	double track;
 };
 
-/* declare the connection routine */
-static int nmea_connect();
+enum type {
+	type_wgs84,
+	type_dms_kmh,
+	type_dms_mph,
+	type_dms_kn,
+	type_COUNT,
+	type_DEFAULT = type_wgs84,
+	type_INVALID = -1
+};
+
+struct event;
+struct period;
+
+struct period {
+	struct period *next;
+	struct event *events;
+	uint32_t period;
+	uint32_t last;
+};
+
+struct event {
+	struct event *next;
+	struct event *byid;
+	struct period *period;
+	const char *name;
+	struct afb_event event;
+	enum type type;
+	int id;
+};
+
+static const char * const type_NAMES[type_COUNT] = {
+	"WGS84",
+	"DMS.km/h",
+	"DMS.mph",
+	"DMS.kn"
+};
 
 /*
  * the interface to afb-daemon
@@ -73,19 +113,314 @@ static struct gps frames[10];
 static int frameidx;
 static int newframes;
 
-static const char type_wgs84_name[] = "WGS84";
-static const char type_dms_kmh_name[] = "DMS.km/H";
-static const char type_dms_Mh_name[] = "DMS.M/H";
-static const char type_dms_Knh_name[] = "DMS.Kn/H";
+static struct json_object *time_ms;
+static struct json_object *latitude_wgs;
+static struct json_object *longitude_wgs;
+static struct json_object *latitude_dms;
+static struct json_object *longitude_dms;
+static struct json_object *altitude_m;
+static struct json_object *speed_ms;
+static struct json_object *speed_kmh;
+static struct json_object *speed_mph;
+static struct json_object *speed_kn;
+static struct json_object *track_d;
+
+static struct json_object *positions[type_COUNT];
+
+static struct period *periods;
+static struct event *events;
+
+/* declare the connection routine */
+static int nmea_connect();
+
+/*
+ * Creates the JSON representation for Degree Minute Second representation of coordinates
+ */
+static struct json_object *new_dms(double a, int islat)
+{
+	char buffer[50], pos;
+	double D, M;
+
+	if (islat) {
+		if (a >= 0)
+			pos = 'N';
+		else {
+			a = -a;
+			pos = 'S';
+		}
+	} else {
+		if (a <= 180)
+			pos = 'E';
+		else {
+			a = 360 - a;
+			pos = 'W';
+		}
+	}
+	D = floor(a);
+	a = (a - D) * 60;
+	M = floor(a);
+	a = (a - M) * 60;
+	sprintf(buffer, "%d°%d'%.3f\"%c", (int)D, (int)M, a, pos);
+	return json_object_new_string(buffer);
+}
+
+/*
+ * adds the value (with reference count increment) if not null
+ */
+static void addif(struct json_object *obj, const char *name, struct json_object *val)
+{
+	if (val != NULL)
+		json_object_object_add(obj, name, json_object_get(val));
+}
 
-static struct json_object *last_position;
+/*
+ * release the object (put) and reset the pointer to null
+ */
+static void clear(struct json_object **obj)
+{
+	json_object_put(*obj);
+	*obj = NULL;
+}
 
-static struct json_object *position()
+/*
+ * get the last/current position of type
+ */
+static struct json_object *position(enum type type)
 {
+	struct json_object *result;
+	struct gps *g0;
+
+	/* clean on new frame */
 	if (newframes) {
+		clear(&time_ms);
+		clear(&latitude_wgs);
+		clear(&longitude_wgs);
+		clear(&latitude_dms);
+		clear(&longitude_dms);
+		clear(&altitude_m);
+		clear(&speed_ms);
+		clear(&speed_kmh);
+		clear(&speed_mph);
+		clear(&speed_kn);
+		clear(&track_d);
+		clear(&positions[type_wgs84]);
+		clear(&positions[type_dms_kmh]);
+		clear(&positions[type_dms_mph]);
+		clear(&positions[type_dms_kn]);
 		newframes = 0;
 	}
-	return last_position;
+
+	/* get the result */
+	result = positions[type];
+	if (result == NULL) {
+		DEBUG(afbitf, "building position for type %s", type_NAMES[type]);
+
+		/* should build the result */
+		g0 = &frames[frameidx];
+		result = json_object_new_object();
+		if (result == NULL)
+			return NULL;
+		positions[type] = result;
+
+		/* set the result type */
+		json_object_object_add(result, "type", json_object_new_string(type_NAMES[type]));
+
+		/* build time, altitude and track */
+		if (time_ms == NULL && g0->set.time)
+			time_ms = json_object_new_double (g0->time);
+		addif(result, "time", time_ms);
+		if (altitude_m == NULL && g0->set.altitude)
+			altitude_m = json_object_new_double (g0->altitude);
+		addif(result, "altitude", altitude_m);
+		if (track_d == NULL && g0->set.track)
+			track_d = json_object_new_double (g0->track);
+		addif(result, "track", track_d);
+
+		/* build position */
+		switch (type) {
+		default:
+		case type_wgs84:
+			if (latitude_wgs == NULL && g0->set.latitude)
+				latitude_wgs = json_object_new_double (g0->latitude);
+			addif(result, "latitude", latitude_wgs);
+			if (longitude_wgs == NULL && g0->set.longitude)
+				longitude_wgs = json_object_new_double (g0->longitude);
+			addif(result, "longitude", longitude_wgs);
+			break;
+		case type_dms_kmh:
+		case type_dms_mph:
+		case type_dms_kn:
+			if (latitude_dms == NULL && g0->set.latitude)
+				latitude_dms = new_dms (g0->latitude, 1);
+			addif(result, "latitude", latitude_dms);
+			if (longitude_dms == NULL && g0->set.longitude)
+				longitude_dms = new_dms (g0->longitude, 0);
+			addif(result, "longitude", longitude_dms);
+			break;
+		}
+
+		/* build speed */
+		switch (type) {
+		default:
+		case type_wgs84:
+			if (speed_ms == NULL && g0->set.speed)
+				speed_ms = json_object_new_double (g0->speed);
+			addif(result, "speed", speed_ms);
+			break;
+		case type_dms_kmh:
+			if (speed_kmh == NULL && g0->set.speed)
+				speed_kmh = json_object_new_double (g0->speed * METER_PER_SECOND_TO_KILOMETER_PER_HOUR);
+			addif(result, "speed", speed_kmh);
+			break;
+		case type_dms_mph:
+			if (speed_mph == NULL && g0->set.speed)
+				speed_mph = json_object_new_double (g0->speed * METER_PER_SECOND_TO_MILE_PER_HOUR);
+			addif(result, "speed", speed_mph);
+			break;
+		case type_dms_kn:
+			if (speed_kn == NULL && g0->set.speed)
+				speed_kn = json_object_new_double (g0->speed * METER_PER_SECOND_TO_KNOT);
+			addif(result, "speed", speed_kn);
+			break;
+		}
+	}
+
+	return json_object_get(result);
+}
+
+/*
+ * get the event handler of given id
+ */
+static struct event *event_of_id(int id)
+{
+	struct event *r = events;
+	while(r && r->id != id)
+		r = r->byid;
+	return r;
+}
+
+/*
+ * get the event handler for the type and the period
+ */
+static struct event *event_get(enum type type, int period)
+{
+	static int id;
+	int mask;
+	uint32_t perio;
+	struct period *p, **pp, *np;
+	struct event *e;
+
+	/* normalize the period */
+	period = period <= 100 ? 1 : period > 60000 ? 600 : (period / 100);
+	mask = 31;
+	while(period > (period & mask))
+		mask <<= 1;
+	perio = (uint32_t)(100 * (period & mask));
+
+	/* search for the period */
+	pp = &periods;
+	p = *pp;
+	while(p != NULL && p->period < perio) {
+		pp = &p->next;
+		p = *pp;
+	}
+
+	/* create the period if it misses */
+	if (p == NULL || p->period != perio) {
+		np = calloc(1, sizeof *p);
+		if (np == NULL)
+			return NULL;
+		np->next = p;
+		np->period = perio;
+		*pp = np;
+		p = np;
+	}
+
+	/* search the type */
+	e = p->events;
+	while(e != NULL && e->type != type)
+		e = e->next;
+
+	/* creates the type if needed */
+	if (e == NULL) {
+		e = calloc(1, sizeof *e);
+		if (e == NULL)
+			return NULL;
+
+		e->name = "GPS"; /* TODO */
+		e->event = afb_daemon_make_event(afbitf->daemon, e->name);
+		if (e->event.itf == NULL) {
+			free(e);
+			return NULL;
+		}
+
+		e->next = p->events;
+		e->byid = events;
+		e->period = p;
+		e->type = type;
+		do {
+			id++;
+			if (id < 0)
+				id = 1;
+		} while(event_of_id(id) != NULL);
+		e->id = id;
+		p->events = e;
+		events = e;
+	}
+
+	return e;
+}
+
+static void event_send()
+{
+	struct period *p, **pp;
+	struct event *e, **pe, **peid;
+	struct timeval tv;
+	uint32_t now;
+
+	/* skip if nothing is new */
+	if (!newframes)
+		return;
+
+	/* computes now */
+	gettimeofday(&tv, NULL);
+	now = (uint32_t)(tv.tv_sec * 1000) + (uint32_t)(tv.tv_usec / 1000);
+
+	/* iterates over the periods */
+	pp = &periods;
+	p = *pp;
+	while (p != NULL) {
+		if (p->events == NULL) {
+			/* no event for the period, frees it */
+			*pp = p->next;
+			free(p);
+		} else {
+			if (p->period <= now - p->last) {
+				/* its time to refresh */
+				p->last = now;
+				pe = &p->events;
+				e = *pe;
+				while (e != NULL) {
+					/* sends the event */
+					if (afb_event_push(e->event, position(e->type)) != 0)
+						pe = &e->next;
+					else {
+						/* no more listeners, free the event */
+						*pe = e->next;
+						peid = &events;
+						while (*peid != e)
+							peid = &(*peid)->byid;
+						*peid = e->byid;
+						afb_event_drop(e->event);
+						free(e);
+					}
+					e = *pe;
+				}
+			}
+			pp = &p->next;
+		}
+		p = *pp;
+	}
 }
 
 static int nmea_time(const char *text, uint32_t *result)
@@ -163,7 +498,7 @@ static int nmea_angle(const char *text, double *result)
 		return 0;
 	}
 
-	*result = (double)x + v * 0.01666666666666666666666;
+	*result = (double)x + v * 0.01666666666666666666666; /* 1 / 60 */
 
 	return 1;
 }
@@ -249,6 +584,15 @@ static int nmea_set(
 	frames[frameidx] = gps;
 	newframes++;
 
+	DEBUG(afbitf, "time:%d=%d latitude:%d=%g longitude:%d=%g altitude:%d=%g speed:%d=%g track:%d=%g",
+		(int)gps.set.time, gps.set.time ? (int)gps.time : 0,
+		(int)gps.set.latitude, gps.set.latitude ? gps.latitude : 0,
+		(int)gps.set.longitude, gps.set.longitude ? gps.longitude : 0,
+		(int)gps.set.altitude, gps.set.altitude ? gps.altitude : 0,
+		(int)gps.set.speed, gps.set.speed ? gps.speed : 0,
+		(int)gps.set.track, gps.set.track ? gps.track : 0
+	);
+
 	return 1;
 }
 
@@ -367,8 +711,10 @@ static int nmea_read(int fd)
 static int nmea_on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
 {
 	/* read available data */
-	if ((revents & EPOLLIN) != 0)
+	if ((revents & EPOLLIN) != 0) {
 		nmea_read(fd);
+		event_send();
+	}
 
 	/* check if error or hangup */
 	if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0) {
@@ -376,6 +722,7 @@ static int nmea_on_event(sd_event_source *s, int fd, uint32_t revents, void *use
 		close(fd);
 		nmea_connect(fd);
 	}
+
 	return 0;
 }
 
@@ -443,11 +790,38 @@ static int nmea_connect()
 }
 
 /*
+ * Returns the type corresponding to the given name
+ */
+static enum type type_of_name(const char *name)
+{
+	enum type result;
+	if (name == NULL)
+		return type_DEFAULT;
+	for (result = 0 ; result != type_COUNT ; result++)
+		if (strcmp(type_NAMES[result], name) == 0)
+			return result;
+	return type_INVALID;
+}
+
+/*
+ * extract a valid type from the request
+ */
+static int get_type_for_req(struct afb_req req, enum type *type)
+{
+	if ((*type = type_of_name(afb_req_value(req, "type"))) != type_INVALID)
+		return 1;
+	afb_req_fail(req, "unknown-type", NULL);
+	return 0;
+}
+	
+/*
  * Get the last known position
  */
 static void get(struct afb_req req)
 {
-	afb_req_success(req, position(), NULL);
+	enum type type;
+	if (get_type_for_req(req, &type))
+		afb_req_success(req, position(type), NULL);
 }
 
 /*
@@ -455,7 +829,25 @@ static void get(struct afb_req req)
  */
 static void subscribe(struct afb_req req)
 {
-	afb_req_success(req, NULL, NULL);
+	enum type type;
+	const char *period;
+	struct event *event;
+	struct json_object *json;
+
+	if (get_type_for_req(req, &type)) {
+		period = afb_req_value(req, "period");
+		event = event_get(type, period == NULL ? 2000 : atoi(period));
+		if (event == NULL)
+			afb_req_fail(req, "out-of-memory", NULL);
+		else if (afb_req_subscribe(req, event->event) != 0)
+			afb_req_fail_f(req, "failed", "afb_req_subscribe returned an error: %m");
+		else {
+			json = json_object_new_object();
+			json_object_object_add(json, "name", json_object_new_string(event->name));
+			json_object_object_add(json, "id", json_object_new_int(event->id));
+			afb_req_success(req, json, NULL);
+		}
+	}
 }
 
 /*
@@ -463,7 +855,21 @@ static void subscribe(struct afb_req req)
  */
 static void unsubscribe(struct afb_req req)
 {
-	afb_req_success(req, NULL, NULL);
+	const char *id;
+	struct event *event;
+
+	id = afb_req_value(req, "id");
+	if (id == NULL)
+		afb_req_fail(req, "missing-id", NULL);
+	else {
+		event = event_of_id(atoi(id));
+		if (event == NULL)
+			afb_req_fail(req, "bad-id", NULL);
+		else {
+			afb_req_unsubscribe(req, event->event);
+			afb_req_success(req, NULL, NULL);
+		}
+	}
 }
 
 /*