/*
 * gps2udp
 *
 * Dump NMEA to UDP socket for AIShub
 *      gps2udp -u data.aishub.net:1234
 *
 * Author: Fulup Ar Foll (directly inspired from gpspipe.c)
 * Date:   2013-03-01
 *
 * This file is Copyright by the GPSD project
 * SPDX-License-Identifier: BSD-2-clause
 *
 */

#include "../include/gpsd_config.h"  // must be before all includes

#include <arpa/inet.h>
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#ifdef HAVE_GETOPT_LONG
   #include <getopt.h>   // for getopt_long()
#endif
#include <netinet/in.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
// do not use strsep() it is not POSIX
#include <string.h>      // for strlcpy(), strtok(), etc.
#include <strings.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>

#include "../include/gpsd.h"
#include "../include/gpsdclient.h"
#include "../include/strfuncs.h"
#include "../include/timespec.h"

#define MAX_TIME_LEN 80
#define MAX_GPSD_RETRY 10

static struct gps_data_t gpsdata;

// UDP socket variables
#define MAX_UDP_DEST 5
static int sock[MAX_UDP_DEST];
static int udpchannel;

// gpsclient source
static struct fixsource_t gpsd_source;
static unsigned int flags;
static unsigned int debug = 0;
static bool aisonly = false;
static bool tpvonly = false;

// return local time hh:mm:ss
static char* time2string(void)
{
   static char buffer[MAX_TIME_LEN];
   time_t curtime;
   struct tm *loctime;

   // Get the current time.
   curtime = time(NULL);

   // Convert it to local time representation.
   loctime = localtime(&curtime);

   // Print it out in a nice format.
   (void)strftime(buffer, sizeof(buffer), "%H:%M:%S", loctime);

   return buffer;
}

static int send_udp(char *nmeastring, size_t ind)
{
    char message[MAX_PACKET_LENGTH];
    char *buffer;
    int  channel;
    static char mstr[] = "{\"class\":\"TPV\",";

    // if string length is unknown make a copy and compute it
    if (0 == ind) {
        // compute message size and add 0x0a 0x0d
        for (ind=0; nmeastring [ind] != '\0'; ind ++) {
            if ((sizeof(message) - 3) <= ind) {
                (void)fprintf(stderr, "gps2udp: too big [%s] \n", nmeastring);
                return -1;
            }
            message[ind] = nmeastring[ind];
        }
        buffer = message;
    } else {
        // use directly nmeastring but change terminition
        buffer = nmeastring;
        ind--;
    }
    // Add termination to NMEA feed for AISHUB
    buffer[ind++] = '\r';
    buffer[ind++] = '\n';
    buffer[ind] = '\0';

    if (0 == (flags & WATCH_JSON) &&
        '{' == buffer[0]) {
        // do not send JSON when not configured to do so
        // JSON, skip it
        if (1 < debug) {
            (void)fprintf(stdout, "...j [%s] '%s'\n", time2string(), buffer);
        }
        return 0;
    }
    if (tpvonly &&
        0 != strncmp(mstr, buffer, sizeof(mstr) - 1)) {
        // only TPV requests, but not TPV, skip it
        if (1 < debug) {
            (void)fprintf(stdout, "...t [%s] '%s'\n", time2string(), buffer);
        }
        return 0;
    }

    // send message on udp channel
    for (channel = 0; channel < udpchannel; channel ++) {
        ssize_t status = send(sock[channel],
                              buffer,
                              ind,
                              0);
        if (status < (ssize_t)ind) {
            (void)fprintf(stderr, "gps2udp: failed to send [%s] \n",
                          buffer);
            return -1;
        }
    }
    return 0;
}

// Open and bind udp socket to host
static int open_udp(char **hostport)
{
   int channel;

   for (channel = 0; channel < udpchannel; channel++) {
       char *hostname = NULL;
       char *portname = NULL;

       if (NULL == hostport[channel]) {
           // pacify coverity
           (void)fprintf(stderr, "gps2udp: syntax is [-u hostname:port]\n");
           return -1;
       }
       // parse argument
       hostname = strtok(hostport[channel], ":");
       // NULL tells strtok() to resume search from last found token
       portname = strtok(NULL, ":");
       if ((NULL == hostname) ||
           (NULL == portname)) {
           (void)fprintf(stderr, "gps2udp: syntax is [-u hostname:port]\n");
           return -1;
       }

       sock[channel] = netlib_connectsock1(AF_UNSPEC, hostname, portname, "udp",
                                           0, false, NULL, 0);
       if (0 > sock[channel]) {
           (void)fprintf(stderr, "gps2udp: error creating UDP socket: %s\n",
                         netlib_errstr(sock[channel]));
           return -1;
       }
    }
    return 0;
}

static void usage(void)
{
    (void)fprintf(stderr,
                  "Usage: gps2udp [OPTIONS] [server[:port[:device]]]\n\n"
#ifdef HAVE_GETOPT_LONG
                  "  -?                  Show this help, then exit\n"
                  "  --ais               Select AIS messages only.\n"
                  "  --count COUNT       exit after count packets.\n"
                  "  --daemon            Daemonize\n"
                  "  --debug DEBUGLEVEL  See -d for DEBUGLEVEL\n"
                  "  --help              Show this help, then exit\n"
                  "  --json              Feed JSON messages only.\n"
                  "  --nmea              Feed NMEA messages only.\n"
                  "  --tpv               Feed TPV JSON messages only.\n"
                  "                      Implies --json.\n"
                  "  --udp HOST:PORT     Send UDP feed to host:port.\n"
                  "                      Up to five --udp accepted.\n"
                  "  --version           Show version, then exit\n"
#endif
                  "  -a                  Select AIS messages only.\n"
                  "  -b                  Run in background as a daemon.\n"
                  "  -c COUNT            Exit after count packets.\n"
                  "  -d [0-2]            1 display sent packets, "
                  "2 display ignored packets.\n"
                  "  -h                  Show this help.\n"
                  "  -j                  Feed JSON.\n"
                  "  -n                  Feed NMEA.\n"
                  "  -t                  Feed TPV JSON messages only.\n"
                  "                      Implies --json.\n"
                  "  -u HOST:PORT        Send UDP NMEA/JSON feed to "
                  "host:port.\n"
                  "                      Up to five -u accepted.\n"
                  "  -V                  Print version and exit.\n"
                  "\n"
                  "example: gps2udp -a -n -c 2 -d 1 -u data.aishub.net:2222 "
                  "fridu.net\n"
                  );
}

// loop until we connect with gpsd
static void connect2gpsd(bool restart)
{
    unsigned int delay;

    if (restart) {
        (void)gps_close(&gpsdata);
        if (0 < debug) {
            (void)fprintf(stdout,
                          "gps2udp [%s] reset gpsd connection\n",
                          time2string());
        }
    }

    /* loop until we reach GPSd
     * start at 10 seconds delay, no more than 1 hour (3600 seconds)
     */
    delay = 10;
    while (1) {
        int err = gps_open(gpsd_source.server, gpsd_source.port, &gpsdata);
        if (0 != err) {
            (void)fprintf(stderr,
                          "gps2udp [%s] gps_open(%s, %s) failed "
                          " %s(%d) errno %s(%d)\n",
                          time2string(), gpsd_source.server, gpsd_source.port,
                          gps_errstr(err), err, strerror(errno), errno);
           (void)sleep(delay);
        } else {
            if (0 < debug) {
                (void)fprintf(stdout, "gps2udp [%s] connect to gpsd %s:%s\n",
                              time2string(), gpsd_source.server,
                              gpsd_source.port);
            }
            break;
        }
        delay *= 2;
        if (3600 < delay) {
            // one hour wait max. Fix Coverity 498038, delay went infinite.
            delay = 3600;
        }
    }
    // select the right set of gps data
    (void)gps_stream(&gpsdata, flags, gpsd_source.device);
}

// get data from gpsd
static ssize_t read_gpsd(char *message, size_t len)
{
    int ind;
    char c;
    int retry = 0;
    struct timespec to;

    // allow room for trailing NUL
    len--;

    // loop until we get some data or an error
    for (ind = 0; ind < (int)len;) {
        // prepare for a blocking read with a 10s timeout
        to.tv_sec = 10;
        to.tv_nsec = 0;
        int result = nanowait(gpsdata.gps_fd, &to);

        switch (result) {
        case 1:
            // we have data waiting, let's process them
            // FIXME!  Do not do one at a time!
            // Flawfinder: ignore
            result = (int)read(gpsdata.gps_fd, &c, 1);

            // If we lost gpsd connection reset it
            if (1 != result ) {
                connect2gpsd (true);
            }

            if (('\n' == c) ||
                ('\r' == c)) {
                message[ind]='\0';

                if (0 < ind) {
                    if (0 < retry) {
                        if (1 == debug) {
                            (void)fprintf (stdout,"\r");
                        } else if (1 < debug) {
                            (void)fprintf(stdout,
                                          " [%s] No Data for: %ds\n",
                                          time2string(), retry*10);
                        }
                    }

                    if (tpvonly &&
                        '{' != message[0]) {
                        if (1 < debug) {
                            (void)fprintf(stdout,
                                          "...{ [%s %d] '%s'\n", time2string(),
                                          ind, message);
                        }
                        return 0;
                    }
                    if (aisonly &&
                        '!' != message[0]) {
                        if (1 < debug) {
                            (void)fprintf(stdout,
                                          "...! [%s %d] '%s'\n", time2string(),
                                          ind, message);
                        }
                        return 0;
                    }
                }

                return (ssize_t)ind + 1;
            } else {
                message[ind]= c;
                ind++;
            }
            break;

        case 0: // no data fail in timeout
            retry++;
            // if too many empty packets are received reset gpsd connection
            if (MAX_GPSD_RETRY < retry) {
                connect2gpsd(true);
                retry = 0;
            }
            if (0 < debug) {
                ignore_return(write (1, ".", 1));
            }
            break;

        default:        // we lost connection with gpsd
            connect2gpsd(true);
            break;
        }
    }
    message[ind] = '\0';
    (void)fprintf (stderr,"\n gps2udp: message too big [%s]\n", message);
    return -1;
}

// 6 bits decoding of AIS payload
static unsigned char AISto6bit(unsigned char c)
{
    unsigned char cp = c;

    if((unsigned char)0x30 > c) {
        return (unsigned char)-1;
    }
    if((unsigned char)0x77 < c) {
        return (unsigned char)-1;
    }
    if(((unsigned char)0x57 < c) &&
       ((unsigned char)0x60 > c)) {
        return (unsigned char)-1;
    }

    cp += (unsigned char)0x28;

    if((unsigned char)0x80 < cp) {
        cp += (unsigned char)0x20;
    } else {
        cp += (unsigned char)0x28;
    }
    return (unsigned char)(cp & (unsigned char)0x3f);
}

// get MMSI from AIS bit string
static unsigned int AISGetInt(unsigned char *bitbytes, unsigned int sp,
                              unsigned int len)
{
    unsigned int acc = 0;
    unsigned int s0p = sp-1;                          // to zero base
    unsigned int i;

    for(i = 0; i < len; i++) {
        unsigned int cp, cx, c0;

        acc  = acc << 1;
        cp = (s0p + i) / 6;
        cx = (unsigned int)bitbytes[cp];      // what if cp >= byte_length?
        c0 = (cx >> (5 - ((s0p + i) % 6))) & 1;
        acc |= c0;
    }

    return acc;
}

int main(int argc, char **argv)
{
    bool daemonize = false;
    long count = -1;
    char *udphostport[MAX_UDP_DEST];
    const char *optstring = "?abc:d:hjntu:V";
#ifdef HAVE_GETOPT_LONG
    int option_index = 0;
    static struct option long_options[] = {
        {"ais", no_argument, NULL, 'a'},
        {"count", required_argument, NULL, 'c'},
        {"daemon", no_argument, NULL, 'b'},
        {"debug", required_argument, NULL, 'd'},
        {"help", no_argument, NULL, 'h'},
        {"json", no_argument, NULL, 'j'},
        {"nmea", no_argument, NULL, 'n'},
        {"udp", required_argument, NULL, 'u'},
        {"version", no_argument, NULL, 'V' },
        {NULL, 0, NULL, 0},
    };
#endif

    // pacify covarity
    memset(udphostport, 0, sizeof(udphostport));

    flags = WATCH_ENABLE;
    while (1) {
        int ch;

#ifdef HAVE_GETOPT_LONG
        ch = getopt_long(argc, argv, optstring, long_options, &option_index);
#else
        ch = getopt(argc, argv, optstring);
#endif
        if (-1 == ch) {
            break;
        }

        switch (ch) {
        case 'a':
            aisonly = true;
            if (0 < debug) {
                (void)fprintf(stdout, "AIS only selected\n");
            }
            break;
        case 'b':
            daemonize = true;
            if (0 < debug) {
                (void)fprintf(stdout, "Daemonize selected\n");
            }
            break;
        case 'c':
            count = atol(optarg);
            if (0 < debug) {
                (void)fprintf(stdout, "Count %ld selected\n", count);
            }
            break;
        case 'd':
            debug = atoi(optarg);
            if (2 < debug) {
                usage();
                exit(1);
            }
            if (0 < debug) {
                (void)fprintf(stdout, "Debug %u selected\n", debug);
            }
            break;
        case 'j':
            if (0 < debug) {
                (void)fprintf(stdout, "JSON selected\n");
            }
            flags |= WATCH_JSON;
            break;
        case 'n':
            if (0 < debug) {
                (void)fprintf(stdout, "NMEA selected\n");
            }
            flags |= WATCH_NMEA;
            break;
        case 't':
            if (0 < debug) {
                (void)fprintf(stdout, "TPV and JSON selected\n");
            }
            flags |= WATCH_JSON;
            tpvonly = true;
            break;
        case 'u':
            if (MAX_UDP_DEST <= udpchannel) {
                (void)fprintf(stderr,
                              "gps2udp: too many UDP destinations (max=%d).\n",
                              MAX_UDP_DEST);
            } else {
                udphostport[udpchannel++] = optarg;
                if (0 < debug) {
                    (void)fprintf(stdout, "UDP %s added.\n", optarg);
                }
            }
            break;
        case '?':
            FALLTHROUGH
        case 'h':
            FALLTHROUGH
        default:
            usage();
            exit(1);
        case 'V':
            (void)fprintf(stderr, "%s: %s (revision %s)\n",
                          argv[0], VERSION, REVISION);
            exit(0);
        }
    }

    // Grok the server, port, and device.
    if (optind < argc) {
        gpsd_source_spec(argv[optind], &gpsd_source);
    } else {
        gpsd_source_spec(NULL, &gpsd_source);
    }
    if (NULL == gpsd_source.device) {
        if (0 < debug) {
            (void)fprintf(stdout, "gpsd source %s:%s\n",
            gpsd_source.server, gpsd_source.port);
        }
    } else {
        flags |= WATCH_DEVICE;
        if (0 < debug) {
            (void)fprintf(stdout, "gpsd source %s:%s:%s\n",
            gpsd_source.server, gpsd_source.port, gpsd_source.device);
        }
    }

    // check before going background if we can connect to gpsd
    connect2gpsd(false);

    // Open UDP port
    if (0 < udpchannel) {
        int status = open_udp(udphostport);

        if (0 != status) {
            exit(1);
        }
    }

    // Daemonize if the user requested it.
    if (daemonize) {
        if (0 != os_daemon(0, 0)) {
            (void)fprintf(stderr,
                          "gps2udp: daemonization failed: %s\n",
                          strerror(errno));
        }
    }

    // infinite loop to get data from gpsd and push them to aggregators
    for (;;) {
        char buffer[MAX_PACKET_LENGTH];
        ssize_t  len;

        len = read_gpsd(buffer, sizeof(buffer));

        // ignore empty message
        if (3 < len) {
            if (0 < debug) {
                (void)fprintf (stdout,"---> [%s] -- %s",time2string(),buffer);

                // Try to extract MMSI from AIS payload
                if (str_starts_with(buffer, "!AIVDM")) {
#define MAX_INFO 6
                    int  i, j;
                    char packet[MAX_PACKET_LENGTH];
                    char *adrpkt = packet;
                    unsigned char *info[MAX_INFO];
                    unsigned int  mmsi;
                    unsigned char bitstrings[255];
                    int info_5_len;

                    // pacify coverity.
                    memset(bitstrings, 0, sizeof(bitstrings));

                    // strtok break original string
                    (void)strlcpy(packet, buffer, sizeof(packet));
                    for (j = 0; j < MAX_INFO; j++) {
                        info[j] = (unsigned char *)strtok(adrpkt, ",");
                        // have strtok() continue from last position
                        adrpkt = NULL;
                    }

                    // codacy does not like strlen()
                    info_5_len = (int)strnlen((char *)info[5],
                                              sizeof(bitstrings));
                    if (((int)sizeof(bitstrings) - 1) <= info_5_len) {
                        info_5_len = (int)sizeof(bitstrings) - 1;
                    }
                    for(i = 0 ; i < info_5_len; i++)  {
                        bitstrings[i] = AISto6bit(info[5][i]);
                    }

                    mmsi = AISGetInt(bitstrings, 9, 30);
                    (void)fprintf(stdout," MMSI=%9u", mmsi);

                }
                (void)fprintf(stdout,"\n");
            }

            // send to all UDP destinations
            if (0 < udpchannel) {
                (void)send_udp(buffer, (size_t)len);
            }

            // if we count messages check it now
            if (0 <= count) {
                if (0 == count--) {
                    // completed count
                    (void)fprintf(stderr,
                                  "gpsd2udp: normal exit after counted "
                                  "packets\n");
                    exit (0);
                }
            }  // end count
        } // end len > 3
    } // end for (;;)

    // This is an infinite loop, should never be here
    // coverity[unreachable]
    (void)fprintf (stderr, "gpsd2udp ERROR abnormal exit\n");
    exit (-1);
}

// vim: set expandtab shiftwidth=4
