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

#include "libcommon/error.h"
#include "libcommon/timestamp.h"
#include "libcommon/getopt.h"
#include "libcommon/units.h"
#include "libcommon/generic_sensor_driver.h"

#include "lcmtypes/senlcm_kvh_dsp3000_t.h"


static int
myopts (generic_sensor_driver_t *gsd)
{
    getopt_add_description (gsd->gopt, "KVH DSP3000 FOG sensor driver.");
    getopt_add_string (gsd->gopt, 'm', "mode", "rate", "Output mode {rate, delta, angle}");
    getopt_add_bool   (gsd->gopt, 'z', "zero", 0,      "Zero tare");
    return 0;
}


int
main (int argc, char *argv[])
{
    generic_sensor_driver_t *gsd = gsd_create (argc, argv, NULL, myopts);
    gsd_canonical (gsd, '\r','\n');
    gsd_launch (gsd);

    /* init kvh mode */
    int mode;
    const char *mode_str = getopt_get_string (gsd->gopt, "mode");
    if (!strcasecmp (mode_str, "rate")) {
        mode = SENLCM_KVH_DSP3000_T_RATE_MODE;
        gsd_write (gsd, "R", 1);
    }
    else if (!strcasecmp (mode_str, "delta")) {
        mode = SENLCM_KVH_DSP3000_T_DELTA_MODE;
        gsd_write (gsd, "A", 1);
    }
    else if (!strcasecmp (mode_str, "angle")) {
        mode = SENLCM_KVH_DSP3000_T_ANGLE_MODE;
        gsd_write (gsd, "P", 1);
    }
    else {
        ERROR ("unknown mode option");
        exit (EXIT_FAILURE);
    }

    if (getopt_get_bool (gsd->gopt, "zero"))
        gsd_write (gsd, "Z", 1);

    gsd_flush (gsd);
    gsd_reset_stats (gsd);
    while (1) {
        // read stream
        char buf[128];
        int64_t timestamp;
        gsd_read (gsd, buf, 128, &timestamp);

        double data;
        int valid;
        int ret = sscanf (buf, "%lf %d", &data, &valid);
        if (ret == 2 && valid == 1) {
            senlcm_kvh_dsp3000_t kvh;
            kvh.utime = timestamp;
            kvh.mode = mode;
            kvh.data = data * DTOR; // rad or rad/s depending on mode
            /* Earth rate in degrees = -15.04107 x sin(latitude) Note:
               Northern latitudes are positive and southern latitudes
               are negative.
            */

            senlcm_kvh_dsp3000_t_publish (gsd->lcm, gsd->channel, &kvh);
            gsd_update_stats (gsd, 1);
        }
        else {
            gsd_update_stats (gsd, -1);
        }
    } // while
}
