#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <libgen.h>
#include <ctype.h>
#include <lcm/lcm.h>

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

#include "handlers.h"
#include "textread.h"

#define MAX_SUBSCRIBERS 100

static void
add_subscriptions (lcm_t *lcm, textread_t *tra[])
{   
    textread_t **tra0 = tra;
    // add your subscriptions here
    senlcm_os_compass_t_subscribe (lcm, "OS_COMPASS", &senlcm_os_compass_t_handler, *tra++);
    senlcm_kvh_dsp3000_t_subscribe (lcm, "KVH", &senlcm_kvh_dsp3000_t_handler, *tra++);
    senlcm_dstar_ssp1_t_subscribe (lcm, "DESERT_STAR", &senlcm_dstar_ssp1_t_handler, *tra++);
    senlcm_rdi_pd4_t_subscribe (lcm, "RDI", &senlcm_rdi_pd4_t_handler, *tra++);
    senlcm_ms_gx1_t_subscribe (lcm, "MICROSTRAIN", &senlcm_ms_gx1_t_handler, *tra++);
    senlcm_ppsboard_t_subscribe (lcm, "PPSBOARD", &senlcm_ppsboard_t_handler, *tra++);
    senlcm_gpsd_t_subscribe (lcm, "GPSD_CLIENT", &senlcm_gpsd_t_handler, *tra);
    if (tra - tra0 >= MAX_SUBSCRIBERS) {
        ERROR ("overstepping textread array (tra) boundaries, increase MAX_SUBSCRIBERS");
        abort ();
    }
}


int
main (int argc, char *argv[])
{
    // add options
    getopt_t *gopt = getopt_create ();
    getopt_add_description (gopt, "Export a lcm log file to .csv format for a predefined subset of lcmtypes");
    getopt_add_help   (gopt, NULL);
    getopt_add_bool   (gopt, 'm', "matlab", 0,        "Autogenerate a matlab loader script");
    getopt_add_string (gopt, 'd', "dir",    "./",     "Output directory for .csv's");
    getopt_add_string (gopt, 'p', "prefix", "LCMLOG", "Output filename prefix");

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

    // process options
    const char *csvlog_dir = getopt_get_string (gopt, "dir");
    stdiou_mkpath (csvlog_dir, 0775);

    char *lcmlog_fullname = g_ptr_array_index (gopt->extraargs, 0);
    
    const char *lcmlog_dir = dirname (strdup (lcmlog_fullname));
    const char *lcmlog_fname = basename (strdup (lcmlog_fullname));

    const char *csvlog_prefix = NULL;
    if (getopt_has_flag (gopt, "prefix"))
        csvlog_prefix = getopt_get_string (gopt, "prefix");
    else
        csvlog_prefix = lcmlog_fname;

    textread_t *tra[MAX_SUBSCRIBERS]; // tr array
    for (int i=0; i<MAX_SUBSCRIBERS; i++)
        tra[i] = textread_create (csvlog_dir, csvlog_prefix);


    // setup lcm log file for playback
    char lcm_url[PATH_MAX];
    snprintf (lcm_url, sizeof lcm_url, "file://%s/%s?speed=0", lcmlog_dir, lcmlog_fname);
    lcm_t *lcm = lcm_create (lcm_url);
    add_subscriptions (lcm, tra);
    while (0==lcm_handle (lcm)) {};

    // generate matlab load script?
    if (getopt_get_bool (gopt, "matlab")) {
        char mfile_fname[NAME_MAX];
        sprintf (mfile_fname, "%s", lcmlog_fname);
        for (int i=0; i<strlen (mfile_fname); i++) {
            if (!isalnum (mfile_fname[i]))
                mfile_fname[i] = '_';
        }
        textread_gen_matlab (tra, MAX_SUBSCRIBERS, csvlog_dir, mfile_fname);
    }

    // clean up
    for (int i=0; i<MAX_SUBSCRIBERS; i++) {
        if (!tra[i]->empty)
            fclose (tra[i]->csvlog_fid);
        textread_destroy (tra[i]);
    }
    lcm_destroy (lcm);

    return 0;
}
