/****************************************************************************/
/* Copyright (c) 2018 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Power Buoy Nav440 serial repeater.                            */
/* Filename : nav440.c                                                      */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 04/30/2018                                                    */
/* Modified : Started with the nanomsg pubsub demo app and modified for use */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

/*
    Copyright 2016 Garrett D'Amore <garrett@damore.org>

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"),
    to deal in the Software without restriction, including without limitation
    the rights to use, copy, modify, merge, publish, distribute, sublicense,
    and/or sell copies of the Software, and to permit persons to whom
    the Software is furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included
    in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
    THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
    IN THE SOFTWARE.

    "nanomsg" is a trademark of Martin Sustrik
*/

/*  This program serves as an example for how to write a simple PUB SUB
    service, The server is just a single threaded for loop which broadcasts
    messages to clients, every so often.  The message is a binary format
    message, containing two 32-bit unsigned integers.  The first is UNIX time,
    and the second is the number of directly connected subscribers.

    The clients stay connected and print a message with this information
    along with their process ID to standard output.

    To run this program, start the server as pubsub_demo <url> -s
    Then connect to it with the client as pubsub_demo <url>
    For example:

    % ./pubsub_demo tcp://127.0.0.1:5555 -s &
    % ./pubsub_demo tcp://127.0.0.1:5555 &
    % ./pubsub_demo tcp://127.0.0.1:5555 &
    11:23:54 <pid 1254> There are 2 clients connected.
    11:24:04 <pid 1255> There are 2 clients connected.
    ..
*/

/* The default Nav440 serial port */

#define XB_PORT "/dev/ttyO4"

#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <netinet/in.h>  /* For htonl and ntohl */
#include <unistd.h>

#include <nanomsg/nn.h>
#include <nanomsg/pubsub.h>

/*  The server runs forever. */
/*  url is the server socket, xb is the file
 *  descriptor to the opened Nav440 serial port */
int server(const char *url, int xb)
{
    int  fd; 
    char recving = 0;

    /*  Create the socket. */
    fd = nn_socket (AF_SP, NN_PUB);
    if (fd < 0) {
        fprintf (stderr, "nn_socket: %s\n", nn_strerror (nn_errno ()));
        return (-1);
    }

    /*  Bind to the URL.  This will bind to the address and listen
        synchronously; new clients will be accepted asynchronously
        without further action from the calling program. */

    if (nn_bind (fd, url) < 0) {
        fprintf (stderr, "nn_bind: %s\n", nn_strerror (nn_errno ()));
        nn_close (fd);
        return (-1);
    }

    /*  Now we can just publish results.  Note that there is no explicit
        accept required.  We just start writing the information. */

    for (;;) {
        char buf[1000];
        uint8_t msg[2 * sizeof (uint32_t)];
        uint32_t secs, subs;
        int rc, nb;

        secs = (uint32_t) time (NULL);
        subs = (uint32_t) nn_get_statistic (fd, NN_STAT_CURRENT_CONNECTIONS);

        secs = htonl (secs);
        subs = htonl (subs);

        memcpy (msg, &secs, sizeof (secs));
        memcpy (msg + sizeof (secs), &subs, sizeof (subs));

        /* Read from the Nav440 port and publish to the nanomsg socket */ 
    	nb = read(xb, buf, sizeof(buf));
        if (nb > 0)
    	{
    	  if (!recving) fprintf(stderr, "Nav440 recving bytes from %s\n", XB_PORT);
    	  recving = 1;
    	}
        else if (nb < 0)
    	{
    	  perror("Error reading from XB_PORT");
    	}

        /* publish any data to subscribers */
        rc = nn_send (fd, buf, nb, 0);
        if (rc < 0) {
            /*  There are several legitimate reasons this can fail.
                We note them for debugging purposes, but then ignore
                otherwise. */
            fprintf (stderr, "nn_send: %s (ignoring)\n",
                nn_strerror (nn_errno ()));
        }
        else
        {
          fprintf(stderr, "Nav440 pushed %d bytes\n", rc);
        }
    }

    /* NOTREACHED */
    nn_close (fd);
    return (-1);
}

/*  The client runs in a loop, displaying the content. */
int client (const char *url)
{
    int fd;
    int rc;

    fd = nn_socket (AF_SP, NN_SUB);
    if (fd < 0) {
        fprintf (stderr, "nn_socket: %s\n", nn_strerror (nn_errno ()));
        return (-1);
    }

    if (nn_connect (fd, url) < 0) {
        fprintf (stderr, "nn_socket: %s\n", nn_strerror (nn_errno ()));
        nn_close (fd);
        return (-1);        
    }

    /*  We want all messages, so just subscribe to the empty value. */
    if (nn_setsockopt (fd, NN_SUB, NN_SUB_SUBSCRIBE, "", 0) < 0) {
        fprintf (stderr, "nn_setsockopt: %s\n", nn_strerror (nn_errno ()));
        nn_close (fd);
        return (-1);        
    }

    for (;;) {
      char buf[1000];
        uint8_t msg[2 * sizeof (uint32_t)];
        char hhmmss[9];  /* HH:MM:SS\0 */
        uint32_t subs, secs;
        time_t t;

        rc = nn_recv (fd, buf, sizeof (buf), 0);
        if (rc < 0) {
            fprintf (stderr, "nn_recv: %s\n", nn_strerror (nn_errno ()));
            break;
        }
    	else
        {
            fprintf (stderr, "Received %d bytes\n", rc);
            sleep(1);
        }
    }

    nn_close (fd);
    return (-1);
}

void usage()
{
    fprintf (stderr, "Usage: nav440 <url> [-p /dev/port -s]\n");
    fprintf (stderr, "  <url> nanomsg url (e.g., ipc:///tmp/nav440.ipc)\n");
    fprintf (stderr, "  -p    Nav440 serial port (default is /dev/ttyO4)\n");
    fprintf (stderr, "  -s    Server mode\n");
}

/* Main
   Parse command line and run the server or client
*/
int main (int argc, char **argv)
{
    int _fd0, _fd1, i, serv;
    char *nav440_port = XB_PORT;

    /* Gotta be at least one argument (the nanomsg url */
    if (argc < 2)
    {
        usage();
        exit(0);
    }

    /* Parse the options:
       server/client
       Nav440 serial port
    */
    for (serv = 0, i = 2; i < argc; i++)
    {
        /* Get the server option */
        if (0 == strcmp(argv[i], "-s"))
        {
            serv = 1;
        }
        /* Get the serial port argument */
        else if (0 == strcmp(argv[i], "-p"))
        {
            /* Serial port must be the next argument */
            if (argc > i+1)
            {
                nav440_port = strdup(argv[i+1]);
            }
            else
            {
                usage();
                exit(0);
            }
        }
    }

    /* Attempt to open the Nav440 port here */
    if ( (_fd0 = open(nav440_port, O_RDONLY | O_NOCTTY)) < 0 )
    {
       /* Could not open the port. */
       fprintf(stderr, "nav440: Unable to open serial port %s\n", nav440_port);
       perror("nav440");
       usage();
       exit(0);
    }

    int rc;
    /* Run the application in the desired mode */
    if (serv)
    {
       rc = server (argv[1], _fd0);
    }
    else
    {
	   rc = client (argv[1]);    /* Used as a test only */
    }

    exit (rc == 0 ? EXIT_SUCCESS : EXIT_FAILURE);
}