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

#include "libcommon/getopt.h"
#include "libcommon/pretty_print.h"

#include "lcmtypes/perlcm_raw_t.h"

static void
raw_t_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                const perlcm_raw_t *raw, void *user)
{
    if (raw->dtype == PERLCM_RAW_T_DTYPE_ASCII)
        printf ("CH=%s A %s\n", channel, raw->buf);
    else {
        printf ("CH=%s B ", channel);
        hexdump (raw->data, raw->datalen);
        printf ("\n");
    }
    
}


int main (int argc, char *argv[])
{
    getopt_t *gopt = getopt_create ();

    /* Add command-line options */
    getopt_add_bool (gopt, 'h', "help", 0, "Show this");

    if (!getopt_parse (gopt, argc, argv, 1) || gopt->extraargs->len!=0) {
        getopt_do_usage (gopt);
        exit (EXIT_FAILURE);
    }
    else if (getopt_get_bool (gopt, "help")) {
        getopt_do_usage (gopt);
        exit (EXIT_SUCCESS);
    }

    // create and subscribe to lcm
    lcm_t *lcm = lcm_create (NULL);
    if (!lcm) {
        ERROR ("lcm_create() failed");
        exit (EXIT_FAILURE);
    }
    perlcm_raw_t_subscribe (lcm, "SENSOR_.*_READ", &raw_t_callback, NULL);

    while (1) {
        lcm_handle (lcm);
    }


    return 0;
}
