/* 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 = (int) nb_config_get("gpsd.0.port");
server_port = (int)trim((string) argv[1]);

gpsd = -1;
MAX_CONNECTIONS=10;

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");
    sleep(3);
    nb_config_set("gpsd.0.status=1");
    sleep(5);

    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 <= 5; attempt++) {
        sleep(3);

        if (connect(gpsd, local_ip, local_port) < 0) {
            nb_syslog("Could not connect to daemon");
            if (attempt == 3) {
                nb_syslog("Unable to connect, restarting daemon");
                gpsd_restart();
            }
        } else {
            break;
        }
    }
    nb_syslog("Connected to daemon, requesting NMEA");
    if (nb_config_get("gpsd.0.cmode") == "0") 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();

/* open TCP server socket */
server = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
if (server < 0) {
    nb_syslog("Unable to open tcp socket");
    exit(-1);
}

ret = bind(server, server_port, "");
if (ret == -1) {
    printf("Unable to bind port %d\n", port);
    close(sock);
    exit(1);
}
 
ret = listen(server, MAX_CONNECTIONS);
if (ret == -1) {
    printf("Unable to listen\n");
      close(sock);
      exit(1);
}


while (!quit) {
    client = accept(server);
    if (client < 0) {
		nb_syslog("accept failed\n");
        sleep(1);
        continue;
    } else {
        nb_syslog("new client connection: %i",client);
        clients=mkarray(client);
    } 
    clientConnection=true;
    nb_syslog("New client connected\n");


    // Sending NMEA Loop

    /* process NMEA stream */
    nb_syslog("Processing NMEA data");
    while (clientConnection) {
        /* 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(array_merge(clients,mkarray(gpsd,server)), 1);

        if (rv == -1) {
            nb_syslog("ERROR: select failed, re-connecting");
            close(gpsd);
            gpsd = -1;
            continue;
        } else if (rv == 0) {
            /* nothing received */
            continue;
        } else if(rv == server) {
			newclient = accept(server);
			if (newclient < 0) {
				nb_syslog("accept failed\n");
				continue;
			} else {
                nb_syslog("new client connection: %i",newclient);
                clients=array_merge(clients,mkarray(newclient));

            }
        }
        
        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;
        */ 
        for (i=0;i<length(clients);i++) {
			sent = send(clients[i], data);
			if (sent < 0) {
				nb_syslog("ERROR: Unable to send, removing client (%i)",clients[i]); 
				clients=array_compact(array_unset(clients,i));
			} 
        } 
    }



}

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