/* DESC: This script sends the local GPS NMEA stream to a remote UDP server.
* DESC: It checks for the correct CR/LF end of line characters and convert messages $GN to $GP 
* Copyright (C) 2013-2019) NetModule AG, Switzerland
*/

/* default values */
local_ip = "127.0.0.1";
local_port = 2947; /* would be better to take it from the configuaration */

gpsd = -1;


void usage() {
	printf("usage: gps-udp-client.are <server> <port>\n");
	exit(1);
}

int gpsd_restart ()
{
	nb_syslog("Restarting GPS daemon");

	nb_config_set("gpsd.0.status=0");
                do {
                                sleep(3);
                } while ( nb_config_done() != 0)
     
	nb_config_set("gpsd.0.status=1");
                do {
	                sleep(5);
                } while ( nb_config_done() != 0)
	return 0;
}
int gpsd_connect ()
{
	nb_syslog("Connecting to GPS daemon");

	gpsd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (gpsd < 0) {
		nb_syslog("ERROR: Unable to open socket");
		exit(-1);
	}
	for (attempt = 0; attempt <= 15; attempt++) {
		sleep(15);

		if (connect(gpsd, local_ip, local_port) < 0) {
			nb_syslog("Could not connect to daemon");
			if (attempt == 12) {
				nb_syslog("Unable to connect, restarting daemon");
				gpsd_restart();
			}
		} else {
			break;
		}
	}
	nb_syslog("Connected to daemon, requesting NMEA");
	send(gpsd, "R=1\n");

	nb_syslog("Processing NMEA");

	return gpsd;
}

string checksum(string s)
{
	pos = strchr(s, "*");

  	if (is_void(pos))
		return s; /* no checksum to correct */

	cs = 0;
	chars = explode(s);
        for (i = 1; i < pos; i++) { /* skip $ and *XX */ 
	    c = ord(chars[i]);
            cs ^= c;
        }
        r = sprintf("%s%02X%s", substr(s, 0, pos + 1), cs, substr(s, pos + 3));
	return r;
}

/* main() */

if (argc < 2) {
    usage();
}

SERVER = trim((string) argv[1]);
PORT = (int) argv[2];

/* open UDP server socket */
server = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (server < 0) {
	nb_syslog("Unable to open socket");
	exit(-1);
}

/* process NMEA stream */
nb_syslog("Processing NMEA data");
while(1) {
	/* connect to gpsd */
	if(gpsd < 0) {
		gpsd = gpsd_connect(gpsd);
		if(gpsd < 0) {
			nb_syslog("ERROR: Unable to connect to daemon");
			sleep(5);
			continue;
		}
	}

	/* wait for client socket data  */
	rv = select(gpsd, 3);

	if(rv == -1) {
		nb_syslog("ERROR: select failed, re-connecting");
		close(gpsd);
		gpsd = -1;
		continue;
	} else if(rv == 0) {
		/* nothing received */
		continue;
	}
	
	
	data = recv(gpsd);
	len = strlen(data);
	/* Check for correct framing: CR+LF */
	if(len > 2) {
		if(strrchr(data, "\r") != len-2 || strrchr(data, "\n") != len-1) {
			nb_syslog("ERROR: NMEA framing wrong");
			close(gpsd);
			gpsd = -1;
			gpsd_restart();
			continue;
		}
	}

	if(len == 0) {
		nb_syslog("ERROR: no data, re-connecting");
		close(gpsd);
		gpsd = -1;
		continue;
	}
	/* Convert message $GN to $GP for backward compatibility */
	if(strstr(data, "$GN") == 0) {;
		a_arr = explode(data);
		a_arr[2] = "P";
		data = checksum(implode(a_arr));
	}
	/* skip all messages different from GPGGA 
	* if(strstr(data,"$GPGGA")==())
	* continue;
	*/
	sent = sendto(server, data, SERVER, PORT);
	if(sent != len) {
		nb_syslog("ERROR: Unable to send %d bytes to %s:%d", len, SERVER, PORT);
	}
}

if (gpsd > -1)	{
	close(gpsd);
}
if (server > -1) {
	close(server);
}
exit(-1);