#include <stdio.h>
#include <stdlib.h>

#include "libcommon/getopt.h"
#include "lcmtypes/senlcm_raw_t.h"

static void
my_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
             const senlcm_raw_t *raw, void *user)
{
    /* ascii */
    printf ("A: ");
    for (size_t i=0; i<raw->length; i++)
        printf ("%c", (char) raw->data[i]);
    printf ("\n");

    /* binary */
    printf ("B: ");
    for (size_t i=0; i<raw->length; i++)
        printf ("%02x", (char) raw->data[i]);
    printf ("\n");

}

int
main (int argc, char *argv[])
{
    // so that redirected stdout won't be insanely buffered.
    setvbuf (stdout, (char *) NULL, _IONBF, 0);

    getopt_t *gopt = getopt_create ();
    getopt_add_description (gopt, 
                            "Terminal application for displaying LCM senlcm_raw_t packets from specified channel.\n"
                            "Messages are decoded and displayed in both ASCII and hex.");
    getopt_add_help   (gopt, NULL);
    getopt_add_string (gopt, 'C', "channel", "", "LCM channel name");

    if (!getopt_parse (gopt, argc, argv, 1) || getopt_get_bool (gopt, "help") || argc == 1) {
        getopt_do_usage (gopt, NULL);
        exit (EXIT_FAILURE);
    }

    lcm_t *lcm = lcm_create (NULL);
    senlcm_raw_t_subscribe (lcm, getopt_get_string (gopt, "channel"), &my_callback, NULL);

    while (1)
        lcm_handle (lcm);
}
