/* DESC: This script sends the local GPS NMEA via UDP broadcast to multiple ports 
  * selected by NMEA type.
  * Copyright (C) 2017 NetModule AG, Switzerland
  */
BroadcastIP = "192.168.1.255";

// Connect to GPS Deamon
int connect_gpsd() {
    nb_syslog("Connecting to GPS daemon.");
    sock = -1;
    while(true)
    {
        sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
        if (sock < 0)
        {
            nb_syslog("Unable to open TCP socket. Retrying in 5 seconds");
            sleep(5);
            continue;
        }

        if (connect(sock, "127.0.0.1", 2947) < 0)
        {
            nb_syslog("Could not connect to GPS daemon. Retrying in 5 seconds");
            close(sock);
            sleep(5);
            continue;
        }
        nb_syslog("GPS daemon connected.");
        return(sock);
        break;
    }
}

     

gpsd = connect_gpsd();

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

nb_syslog("GPS Broadcast to %s", BroadcastIP);
// process NMEA stream 
   
nb_syslog("Processing NMEA data");
 
while (true) {
   /* wait for socket data  */
   rv = select(gpsd, 3);
   if (rv > 0) {
      data = recv(gpsd);
      len = strlen(data);
   } else if (rv < 0) {
      nb_syslog("select failed");
      len = -1;
   } else { // nothing received
      continue;
   } 
   if (len <= 0) {
      nb_syslog("Receive failed, re-connecting to GPS daemon");
      close(gpsd);
      gpsd = connect_gpsd();
      if (gpsd < 0)
         exit(-2);
   } else {
      // printf("got >>>%s<<<\n", data);
      while(true)   //handle CR /LF problems and separate data into singel $Gxxx frames
         {
            newlinepos = strstr(data, "\r\n");
            seplen = 0;
            if (newlinepos != ()) {
               seplen = 2;
            } else {
               newlinepos = strstr(data, "\n");
               if (newlinepos != ()) {
                  seplen = 1;
               }
            }
            
            if (seplen != 0) {
               frame = strcat(left(data, newlinepos), "\r\n");
               // printf("frame >>>%s<<<\n", frame);
               if (left(frame, 6) == "$GPRMC") {
                  sendto(server, frame, BroadcastIP, 5125);
                  sendto(server, frame, BroadcastIP, 5126);
                  sendto(server, frame, BroadcastIP, 6060);
               }
               if (left(frame, 6) == "$GPGGA") {
                  sendto(server, frame, BroadcastIP, 5126);
               }
               if (left(frame, 6) == "$GPGSA") {
                  sendto(server, frame, BroadcastIP, 5126);
               }      
               data = substr(data, newlinepos + seplen);
            }
            else
               break;
            
        }   
   }
}
exit(-3);