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

#include "libcommon/daemon.h"
#include "libcommon/getopt.h"
#include "libcommon/error.h"
#include "libcommon/timestamp.h"
#include "libcommon/timeutil.h"

#include <lcmtypes/vanlcm_heartbeat_t.h>

typedef struct timer_table timer_table_t;
struct timer_table {
    char   channel[LCM_MAX_CHANNEL_NAME_LENGTH];
    double hz;
};

timer_table_t tt[] = {
    {.channel = "HEARTBEAT_1HZ", .hz = 1},
    {.channel = "HEARTBEAT_10HZ", .hz = 10},
    {.channel = "HEARTBEAT_100HZ", .hz = 100},
};

lcm_t *lcm = NULL;

int
callback (void *user)
{
    char *channel = user;
    vanlcm_heartbeat_t hb = {.utime = timestamp_now ()};
    vanlcm_heartbeat_t_publish (lcm, channel, &hb);
    return 1;
}

int
main (int argc, char *argv[])
{
    const int n_timers = sizeof tt / sizeof (timer_table_t);

    getopt_t *gopt = getopt_create ();
    getopt_add_description (gopt, 
                            "Heartbeat process publishes LCM heartbeat_t messages at regular intervals.");
    getopt_add_bool (gopt, 'h', "help", 0, "Show this");
    getopt_add_bool (gopt, 'D', "daemon", 0, "Run as daemon?");

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

    if (getopt_get_bool (gopt, "daemon"))
        daemon_fork ();
    

    lcm = lcm_create (NULL);
    if (!lcm) {
        ERROR ("lcm_create()");
        exit (EXIT_FAILURE);
    }

    timeutil_timer_t t[n_timers];
    for (int i=0; i<n_timers; i++) {
        struct timespec spec = timeutil_hz_to_timespec (tt[i].hz);
        t[i] = timeutil_timer_create (spec, callback, tt[i].channel);
    }

    while (1)
        lcm_handle (lcm);
}
