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

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

#include "lcmtypes/senlcm_ms_gx1_t.h"

#include "ms_3dm_gx1.h"

#define INST_MAGFIELD SENLCM_MS_GX1_T_INST_MAGFIELD
#define INST_ACCEL    SENLCM_MS_GX1_T_INST_ACCEL
#define INST_ANGRATE  SENLCM_MS_GX1_T_INST_ANGRATE
#define INST_EULER    SENLCM_MS_GX1_T_INST_EULER
#define INST_M        SENLCM_MS_GX1_T_INST_M
#define INST_Q        SENLCM_MS_GX1_T_INST_Q
#define STAB_MAGFIELD SENLCM_MS_GX1_T_STAB_MAGFIELD
#define STAB_ACCEL    SENLCM_MS_GX1_T_STAB_ACCEL
#define STAB_ANGRATE  SENLCM_MS_GX1_T_STAB_ANGRATE
#define STAB_EULER    SENLCM_MS_GX1_T_STAB_EULER
#define STAB_M        SENLCM_MS_GX1_T_STAB_M
#define STAB_Q        SENLCM_MS_GX1_T_STAB_Q
#define TEMPERATURE   SENLCM_MS_GX1_T_TEMPERATURE


#define DEV_TICKS_PER_SECOND (1.0/0.0065536)


static int
poll_data (generic_sensor_driver_t *gsd, char cmd, int len, char buf[], int64_t *utime)
{
    gsd_noncanonical (gsd, len, 0);
    int ret = 0;
    while (ret == 0) {
        gsd_write (gsd, &cmd, 1);
        ret = gsd_read_timeout (gsd, buf, MICROSTRAIN_MAX_LEN, utime, 1000000);
    }
    return ret;
}

static void
getGyroVector (generic_sensor_driver_t *gsd, timestamp_sync_state_t *tss, senlcm_ms_gx1_t *ms)
{
    char buf[MICROSTRAIN_MAX_LEN];
    int buflen = poll_data (gsd, CMD_GYRO_VECTOR, LEN_GYRO_VECTOR, buf, &ms->utime);
    int ret = ms_parseGyroVector (buf, buflen, ms->sMagField, ms->sAccel, ms->sAngRate, &ms->TimerTicks);
    if (ret > 0) {
        ms->bitmask = STAB_MAGFIELD | STAB_ACCEL | STAB_ANGRATE;
        ms->utime = timestamp_sync (tss, (uint16_t) ms->TimerTicks, ms->utime);

        senlcm_ms_gx1_t_publish (gsd->lcm, gsd->channel, ms);
        gsd_update_stats (gsd, 1);
    }
    else
        gsd_update_stats (gsd, -1);    
}

static void
getInstVector (generic_sensor_driver_t *gsd, timestamp_sync_state_t *tss, senlcm_ms_gx1_t *ms)
{
    char buf[MICROSTRAIN_MAX_LEN];
    int buflen = poll_data (gsd, CMD_INST_VECTOR, LEN_INST_VECTOR, buf, &ms->utime);
    int ret = ms_parseInstVector (buf, buflen, ms->iMagField, ms->iAccel, ms->iAngRate, &ms->TimerTicks);
    if (ret > 0) {
        ms->bitmask = INST_MAGFIELD | INST_ACCEL | INST_ANGRATE;
        ms->utime = timestamp_sync (tss, (uint16_t) ms->TimerTicks, ms->utime);

        senlcm_ms_gx1_t_publish (gsd->lcm, gsd->channel, ms);
        gsd_update_stats (gsd, 1);
    }
    else
        gsd_update_stats (gsd, -1);    
}

static void
getInstQuat (generic_sensor_driver_t *gsd, timestamp_sync_state_t *tss, senlcm_ms_gx1_t *ms)
{
    char buf[MICROSTRAIN_MAX_LEN];
    int buflen = poll_data (gsd, CMD_INST_QUAT, LEN_INST_QUAT, buf, &ms->utime);
    int ret = ms_parseInstQuat (buf, buflen, ms->iQ, &ms->TimerTicks);
    if (ret > 0) {
        ms->bitmask = INST_Q;
        ms->utime = timestamp_sync (tss, (uint16_t) ms->TimerTicks, ms->utime);

        ms_Q_to_M (ms->iQ, ms->iM);
        ms->bitmask |= INST_M;

        ms_M_to_Euler (ms->iM, ms->iEuler);
        ms->bitmask |= INST_EULER;

        senlcm_ms_gx1_t_publish (gsd->lcm, gsd->channel, ms);
        gsd_update_stats (gsd, 1);
    }
    else
        gsd_update_stats (gsd, -1);
}

static void
getGyroQuat (generic_sensor_driver_t *gsd, timestamp_sync_state_t *tss, senlcm_ms_gx1_t *ms)
{
    char buf[MICROSTRAIN_MAX_LEN];
    int buflen = poll_data (gsd, CMD_GYRO_QUAT, LEN_GYRO_QUAT, buf, &ms->utime);
    int ret = ms_parseGyroQuat (buf, buflen, ms->sQ, &ms->TimerTicks);
    if (ret > 0) {
        ms->bitmask = STAB_Q;
        ms->utime = timestamp_sync (tss, (uint16_t) ms->TimerTicks, ms->utime);

        ms_Q_to_M (ms->sQ, ms->sM);
        ms->bitmask |= STAB_M;

        ms_M_to_Euler (ms->sM, ms->sEuler);
        ms->bitmask |= STAB_EULER;

        senlcm_ms_gx1_t_publish (gsd->lcm, gsd->channel, ms);
        gsd_update_stats (gsd, 1);
    }
    else
        gsd_update_stats (gsd, -1);
}


static void
getGyroQuatVector (generic_sensor_driver_t *gsd, timestamp_sync_state_t *tss, senlcm_ms_gx1_t *ms)
{
    char buf[MICROSTRAIN_MAX_LEN];
    int buflen = poll_data (gsd, CMD_GYRO_QUAT_VECTOR, LEN_GYRO_QUAT_VECTOR, buf, &ms->utime);
    int ret = ms_parseGyroQuatVector (buf, buflen, ms->sQ, ms->iMagField, ms->iAccel, ms->sAngRate, &ms->TimerTicks);
    if (ret > 0) {
        ms->bitmask = STAB_Q | INST_MAGFIELD | INST_ACCEL | STAB_ANGRATE;
        ms->utime = timestamp_sync (tss, (uint16_t) ms->TimerTicks, ms->utime);

        ms_Q_to_M (ms->sQ, ms->sM);
        ms->bitmask |= STAB_M;

        ms_M_to_Euler (ms->sM, ms->sEuler);
        ms->bitmask |= STAB_EULER;

        senlcm_ms_gx1_t_publish (gsd->lcm, gsd->channel, ms);
        gsd_update_stats (gsd, 1);
    }
    else
        gsd_update_stats (gsd, -1);
}

static void
getTemperature (generic_sensor_driver_t *gsd, timestamp_sync_state_t *tss, senlcm_ms_gx1_t *ms)
{
    char buf[MICROSTRAIN_MAX_LEN];
    int buflen = poll_data (gsd, CMD_TEMPERATURE, LEN_TEMPERATURE, buf, &ms->utime);
    int ret = ms_parseTemperature (buf, buflen, &ms->Temperature, &ms->TimerTicks);
    if (ret > 0) {
        ms->bitmask = TEMPERATURE;
        ms->utime = timestamp_sync (tss, (uint16_t) ms->TimerTicks, ms->utime);

        senlcm_ms_gx1_t_publish (gsd->lcm, gsd->channel, ms);
        gsd_update_stats (gsd, 1);
    }
    else
        gsd_update_stats (gsd, -1);
}

static int
myopts (generic_sensor_driver_t *gsd)
{
    getopt_add_description (gsd->gopt, "Microstrain 3DM-GX1 AHRS sensor driver.");
    getopt_add_bool (gsd->gopt, '\0', "stabAttitude", 0, "Report stabilized attitude (default)");
    getopt_add_bool (gsd->gopt, '\0', "stabVectors",  0, "Report stabilized vectors (default)");
    getopt_add_bool (gsd->gopt, '\0', "instAttitude", 0, "Report instantaneous attitude (default)");
    getopt_add_bool (gsd->gopt, '\0', "instVectors",  0, "Report instantaneous vectors (default)");
    return 0;
}


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

    timestamp_sync_state_t *tss = timestamp_sync_init (DEV_TICKS_PER_SECOND, 65536, 101.0/100.0);

    senlcm_ms_gx1_t ms;
    memset (&ms, 0, sizeof (ms));

    int all = 1;
    if (getopt_has_flag (gsd->gopt, "stabAttitude") || getopt_has_flag (gsd->gopt, "stabVectors")  ||
        getopt_has_flag (gsd->gopt, "instAttitude") || getopt_has_flag (gsd->gopt, "instVectors"))
        all = 0;

    uint8_t i=0;
    while (1) {        
        if (all || getopt_get_bool (gsd->gopt, "stabAttitude"))
            getGyroQuat (gsd, tss, &ms);

        if (all || getopt_get_bool (gsd->gopt, "stabVectors"))
            getGyroVector (gsd, tss, &ms);

        if (all || getopt_get_bool (gsd->gopt, "instAttitude"))
            getInstQuat (gsd, tss, &ms);

        if (all || getopt_get_bool (gsd->gopt, "instVectors"))
            getInstVector (gsd, tss, &ms);

        if ( (i++ % 10)==0 )
            getTemperature (gsd, tss, &ms);
    }
}
