/*
 * gps2udp
 *
 * Dump NMEA to UDP socket for AIShub 
 *      gpspipe -u data.aishub.net:1234
 *
 * Author Fulup Ar Foll (directly inspired from gpspipe)
 * Date   01-march-2013
 *
 * This file is Copyright (c) 2010 by the GPSD project
 * BSD terms apply: see the file COPYING in the distribution root for details.
 *
 */

#include <sys/types.h>
#include <sys/stat.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <errno.h>
#include <string.h>
#include <strings.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>
#include <sys/time.h>
#ifndef S_SPLINT_S
#include <unistd.h>
#endif /* S_SPLINT_S */

#include "gpsd.h"
#include "gpsdclient.h"
#include "revision.h"

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>


static struct gps_data_t gpsdata;

/* UDP socket variables */
static struct sockaddr_in remote;
static int sock;

/* gpsclient source */
static struct fixsource_t gpsd_source;
static int flags;
static int debug=0;

static int send_udp (char *nmeastring, unsigned int ind) {
   char message [255];
   char *buffer;
   int  status;

   /* if string lenght is unknow make a copy and compute it */
   if (ind == 0) {
     /* compute message size and add 0x0a 0x0d */
     for (ind=0; nmeastring [ind] != '\0'; ind ++) {
        if (ind > sizeof (message)) {
          fprintf(stderr, "gpspipe: too big [%s] \n", nmeastring);
          return -1;
        }
        message[ind] = nmeastring[ind];
     }
     buffer = message;
   } else {
     /* use directly nmeastring but change last 2 byte */
     buffer = nmeastring;
     ind=ind-2;
   }

   /* Add termination to NMEA feed for AISHUB */
   buffer[ind]='\n'; ind ++;
   buffer[ind]='\r'; ind ++;
   buffer[ind]='\0';
   
   status=sendto(sock,buffer,ind,0,&remote,sizeof(remote));
   if (status < (int)ind) {
       fprintf(stderr, "gpspipe: fail to send [%s] \n", nmeastring);
       return -1;
   }

return 0;
}


static int open_udp(char *hostport)
/* Open and bind udp socket to host */
{
   struct hostent *hp;
   char *hostname = NULL;
   char *portname = NULL;
   int  status, portnum;

   /* parse argement */
   hostname = strsep(&hostport, ":");
   portname = strsep(&hostport, ":");
   if ((hostname == NULL) || (portname == NULL)) {
     fprintf(stderr, "gpspipe: syntaxe is [-u hostname:port]\n");
     return (-1);
   }

   portnum= strtol (portname,NULL,0);
   if (errno != 0) {
     fprintf(stderr, "gpspipe: syntaxe is [-u hostname:port] [%s] is not a valid port number\n",portname);
     return (-1);
   }

   sock= socket(AF_INET, SOCK_DGRAM, 0);
   if (sock < 0) {
      fprintf(stderr, "gpspipe: error creating UDP socket\n");
      return (-1);
   }

   remote.sin_family = AF_INET;
   hp = gethostbyname(hostname);
   if (hp==NULL) {
     fprintf(stderr, "gpspipe: syntaxe is [-u hostname:port] [%s] is not a valid hostnamer\n",hostname);
     return (-1);
   }

   bcopy((char *)hp->h_addr, (char *)&remote.sin_addr, hp->h_length);
   remote.sin_port = htons(portnum);

   status = send_udp ("## NMEA UDP feed start ##", 0);
   if (status != 0) {
        fprintf(stderr, "gpspipe: fail to send to [%s:%s]\n",hostname, portname);
     return (-1);
   }
return (0);
}

static void usage(void)
{
    (void)fprintf(stderr,
		  "Usage: gps2udp [OPTIONS] [server[:port[:device]]]\n\n"
		  "-h Show this help.\n" 
                  "-u Send Data to host:port in UDP"
		  "-n Feed NMEA.\n"
		  "-j Feed Jason.\n"
		  "-c [count] exit after count packets.\n"
		  "-b Run in background as a daemon.\n" 
		  "-d debug display packets.\n" 
		  "-v Print version and exit.\n\n"
		  "You must specify one, or more, of -r, -R, or -w\n"
		  );
}

/* loop until we connect with GPSd */
static void connect2gpsd (int restart) {
    int delay,status;

    if (restart) gps_close   (&gpsdata);

    /* loop until we reach GPSd */
    for (delay=10;;delay=delay*2) {
        status = gps_open(gpsd_source.server, gpsd_source.port, &gpsdata);
        if (status != 0) {
	   fprintf(stderr, "gps2udp: fail to connect gpsd %s:%s (%d)\n",
	      gpsd_source.server, gpsd_source.port, errno);
           sleep (delay);
        } else {
          if (debug > 0)  fprintf(stdout, "gps2udp: connected to gpsd %s:%s\n",
              gpsd_source.server, gpsd_source.port);
          break;
        }
    }
    /* select the right set of gps data */
    gps_stream(&gpsdata, flags, gpsd_source.device);
    
}

/* get date from gpsd */
static int read_gpsd (char *message, int len) {
   
   struct timeval tv;
   fd_set fds,master;
   int result,ind;
   char c;

    // prepare select structure */
    FD_ZERO(&master);
    FD_SET(gpsdata.gps_fd, &master);


   /* loop until we get some data or an error */
   for (ind=0; ind<len; ind++) {

        /* prepare for a blocking read with a 10s timeout */
        tv.tv_sec =  10;
        tv.tv_usec = 0;
        memcpy(&fds,&master,sizeof(fd_set));
        result = select(gpsdata.gps_fd+1, &fds, NULL, NULL, &tv);
      
        switch (result) {
        case 1: /* we have data waiting let's process them */
           result = (int)read(gpsdata.gps_fd, &c, 1);

           /* If we lost gpsd connection reset it */
           if (result != 1) {
              connect2gpsd (true);
              return (0);
           }
            
           if (c == '\n') {
               message[ind]='\0';
               return (ind+1);
           } else {
              message[ind]= c;
           }
           break;

        case 0: /* no data fail in timeout */
           if (debug > 0) fprintf (stdout, "gps2udp: no gps data (retry)\n");
           break;

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


int main(int argc, char **argv) {
    bool daemonize = false;
    long count = 0;
    int option, status;

    char *udphostport= NULL;

    flags = WATCH_ENABLE;
    while ((option = getopt(argc, argv, "?hbnjcdvl:u:")) != -1) {

	switch (option) {
	case 'd':
            debug=1;
	    break;
	case 'n':
            if (debug >0) fprintf (stdout, "NMEA selected\n");
	    flags |= WATCH_NMEA;
	    break;
	case 'j':
            if (debug >0) fprintf (stdout, "JASON selected\n");
	    flags |= WATCH_JSON;
	    break;
	case 'c':
	    count = strtol(optarg, 0, 0);
	    break;
	case 'b':
	    daemonize = true;
	    break;
        case 'u':
            udphostport = optarg;
            break;
	case 'v':
	    (void)fprintf(stderr, "%s: %s (revision %s)\n",
			  argv[0], VERSION, REVISION);
	    exit(0);
	case '?':
	case 'h':
	default:
	    usage();
	    exit(1);
	}
    }

    /* Grok the server, port, and device. */
    if (optind < argc) gpsd_source_spec(argv[optind], &gpsd_source);
    else gpsd_source_spec(NULL, &gpsd_source);
    if (gpsd_source.device != NULL) flags |= WATCH_DEVICE;

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

    /* Open UDP port */
    if (udphostport) {
        status = open_udp(udphostport);
	if (status !=0) exit (1);
    }

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

    /* Infinit loop to get date from GPSd and push them to AIShub */
    for (;;) {
       char buffer [1024];
       int  len;

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


       if (len > 0) {
         if (debug > 0) fprintf (stdout,"--> [%d] -- %s\n", len,buffer);
         if (udphostport != NULL) send_udp (buffer, len);
       }

       if (count > 0) {
	  if (count-- < 0) {
	    /* completed count */
            fprintf (stderr, "gpsd2udp normal exit after [%d] packets\n",(int)count);
	    break;
	  }
        }
    }

exit (0);
}