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

#include "libcommon/nmea.h"
#include "libcommon/units.h"
#include "libcommon/generic_sensor_driver.h"

#include "libmath/fasttrig.h"
#include "libmath/gsl_util.h"
#include "libmath/so3.h"
#include "libmath/ssc.h"

#include "lcmtypes/senlcm_raw_t.h"
#include "lcmtypes/senlcm_os_compass_t.h"
#include "lcmtypes/senlcm_ms_gx1_t.h"
#include "lcmtypes/senlcm_dstar_ssp1_t.h"
#include "lcmtypes/senlcm_rdi_pd4_t.h"
#include "lcmtypes/auvlcm_os_conduit_t.h"
#include "lcmtypes/auvlcm_os_conduit_raw_t.h"


#define PUBLISH_OS_CONDUIT_RAW  1
#define USE_MICROSTRAIN_RPH     1
#define USE_DSTAR_DEPTH         1


typedef struct _state_t state_t;
struct _state_t
{
    double rph[3];
    auvlcm_os_conduit_t os_conduit;
    senlcm_dstar_ssp1_t dstar;
    generic_sensor_driver_t *gsd;
};



static void
os_conduit_init (auvlcm_os_conduit_t *os_conduit)
{
    memset (os_conduit, 0, sizeof (*os_conduit));
    os_conduit->dvl.value = strdup ("");
    os_conduit->altimeter.value = strdup ("");
    os_conduit->gps.value = strdup ("");
    os_conduit->compass.value = strdup ("");
}

static void
publish (state_t *state, auvlcm_os_conduit_raw_t *sensor, 
         const int64_t utime, const char msg[])
{
    static char channel[LCM_MAX_CHANNEL_NAME_LENGTH] = { '\0' };
    if (!channel[0]) // first time
        sprintf (channel, "%s.OUT", state->gsd->channel);

#if PUBLISH_OS_CONDUIT_RAW
    state->os_conduit.utime = utime;
    sensor->age = (utime - sensor->utime) / 1000;
    sensor->utime = utime;
    free (sensor->value);
    sensor->value = strdup (msg);
    auvlcm_os_conduit_t_publish (state->gsd->lcm, channel, &state->os_conduit);
#endif
}

/* static void */
/* os_compass_callback (const lcm_recv_buf_t *rbuf, const char *channel,  */
/*                      const senlcm_raw_t *raw, void *user) */
/* { */
/*     generic_sensor_driver_t *gsd = (generic_sensor_driver_t *) user; */

/*     if (raw->dtype != SENLCM_RAW_T_DTYPE_ASCII) { */
/*         ERROR ("remote-helm cannot accept binary data"); */
/*         return; */
/*     } */
/*     gsd_write (gsd, raw->buf, strlen (raw->buf)); */
/*     publish (gsd, &os_conduit.compass, raw->utime, raw->buf); */
/* } */

static void
microstrain_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                      const senlcm_ms_gx1_t *ustrain, void *user)
{
    state_t *state = (state_t *) user;

    char msg[256];

    // only send fresh Euler data
    if (! (ustrain->bitmask & SENLCM_MS_GX1_T_STAB_EULER))
        return;

    // store attitude used for compensated altitude calc
    state->rph[0] = -ustrain->sEuler[0];
    state->rph[1] = -ustrain->sEuler[1];
    state->rph[2] =  ustrain->sEuler[2] + M_PI_2;

    nmea_sprintf (msg, "$C%.1lfP%.1lfR%.1lfT%.1lfD%.2lf*",
                  ustrain->sEuler[2] * RTOD + 180, // deg
                  ustrain->sEuler[1] * RTOD * -1, // deg
                  ustrain->sEuler[0] * RTOD * -1, // deg
                  ustrain->Temperature,   // celsius
                  state->dstar.depth * UNITS_METER_TO_FEET); // feet
    gsd_write (state->gsd, msg, strlen (msg));
    publish (state, &state->os_conduit.compass, ustrain->utime, msg);
}

static void
os_compass_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                     const senlcm_os_compass_t *os_compass, void *user)
{
    state_t *state = (state_t *) user;

    char msg[256];

    double dfs_feet;
    if (USE_DSTAR_DEPTH)
        dfs_feet = state->dstar.depth * UNITS_METER_TO_FEET;
    else
        dfs_feet = os_compass->depth * UNITS_METER_TO_FEET;
    
    // store attitude used for compensated altitude calc
    memcpy (state->rph, os_compass->rph, 3 * sizeof (double));

    nmea_sprintf (msg, "$C%.1lfP%.1lfR%.1lfT%.1lfD%.2lf*",
                  os_compass->rph[2] * RTOD, // deg
                  os_compass->rph[1] * RTOD, // deg
                  os_compass->rph[0] * RTOD, // deg
                  os_compass->T,             // celsius
                  dfs_feet);                 // feet
    gsd_write (state->gsd, msg, strlen (msg));
    publish (state, &state->os_conduit.compass, os_compass->utime, msg);
}

static void
desert_star_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                      const senlcm_dstar_ssp1_t *my_dstar, void *user)
{
    state_t *state = (state_t *) user;
    memcpy (&state->dstar, my_dstar, sizeof (*my_dstar));
}

static void
gpsd_client_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                      const senlcm_raw_t *raw, void *user)
{
    state_t *state = (state_t *) user;
    gsd_write (state->gsd, (char *) raw->data, raw->length);
    publish (state, &state->os_conduit.gps, raw->utime, (char *) raw->data);
}

static void
rdi_pd4_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                  const senlcm_rdi_pd4_t *pd4, void *user)
{
    state_t *state = (state_t *) user;

    char msg[256];
    static int firstime = 1;
    static GSLU_VECTOR_VIEW (x_vs, 6);
    static GSLU_MATRIX_VIEW (R_vs, 3, 3);
    if (firstime) {
        firstime = 0;
        config_get_double_array (state->gsd->cfg, "sensor.rdi.x_vs", x_vs.data, 6);
        x_vs.data[SSC_DOF_R] *= DTOR;
        x_vs.data[SSC_DOF_P] *= DTOR;
        x_vs.data[SSC_DOF_H] *= DTOR;
        so3_rotxyz (R_vs.data, &x_vs.data[SSC_DOF_RPH]);
    }


    // Altitude
    // $SDDBT,#,f,#,M,#,F<*cc> feet, Meters, Fathoms
    if (pd4->altitude < 0) { // invalid
        nmea_sprintf (msg, "$SDDBT,,f,,M,,F*");
        gsd_write (state->gsd, msg, strlen (msg));
    }
    else {
        // RDI beam geometry
        double s, c;
        fsincos (30*DTOR, &s, &c);
        //                      b1, b2, b3, b4
        double bhat_data[] = {  0,  s,  0, -s,
                               -s,  0,  s,  0,
                               -c, -c, -c, -c  };
        gsl_matrix_view bhat = gsl_matrix_view_array (bhat_data, 3, 4);

        GSLU_MATRIX_VIEW (R_wv, 3, 3);
        GSLU_MATRIX_VIEW (R_ws, 3, 3);
        so3_rotxyz (R_wv.data, state->rph);
        gslu_mm (&R_ws.matrix, &R_wv.matrix, &R_vs.matrix);
        int nbeams = 0;
        double altavg = 0, altmin = 1000;
        for (int i=0; i < 4; i++) {
            if (pd4->range[i] > 0) {
                nbeams++;

                gsl_vector_view bi_s = gsl_matrix_column (&bhat.matrix, i);
                gsl_vector_scale (&bi_s.vector, pd4->range[i]);

                GSLU_VECTOR_VIEW (bi_w, 3);
                gslu_mv (&bi_w.vector, &R_ws.matrix, &bi_s.vector);
                
                double bi_alt = bi_w.data[2];
                altavg += bi_alt;
                if (bi_alt < altmin)
                    altmin = bi_alt;
            }
        }            
        altavg /= nbeams;

        nmea_sprintf (msg, "$SDDBT,%.1f,f,%.1f,M,%.1f,F*",
                      altmin * UNITS_METER_TO_FEET,     //feet
                      altmin,                           //meters
                      altmin * UNITS_METER_TO_FATHOM);  //fathoms
        gsd_write (state->gsd, msg, strlen (msg));
        publish (state, &state->os_conduit.altimeter, pd4->utime, msg);
    }

    // Velocity
    // $ODVL,<XSPEED>,<YSPEED>,<TMOUT><*cc>
    if (pd4->btv[0] > SENLCM_RDI_PD4_T_BAD_VELOCITY &&
        pd4->btv[1] > SENLCM_RDI_PD4_T_BAD_VELOCITY &&
        pd4->btv[2] > SENLCM_RDI_PD4_T_BAD_VELOCITY) {// three-beam or better soln

        const int UVC_DVL_TIMEOUT = 30;
        gsl_vector_const_view uvw_s = gsl_vector_const_view_array (pd4->btv, 3);
        GSLU_VECTOR_VIEW (uvw_v, 3);
        gslu_mv (&uvw_v.vector, &R_vs.matrix, &uvw_s.vector);
        
        nmea_sprintf (msg, "$ODVL,%f,%f,%d*", uvw_v.data[0], uvw_v.data[1], UVC_DVL_TIMEOUT);
        gsd_write (state->gsd, msg, strlen (msg));
        publish (state, &state->os_conduit.dvl, pd4->utime, msg);
    }
}

static void *
lcm_thread (void *context)
{
    generic_sensor_driver_t *gsd = (generic_sensor_driver_t *) context;
    while (!gsd->done) {
        lcm_handle (gsd->lcm);
        gsd_update_stats (gsd, 1);
    }
    
    return 0;
}

static int
myopts (generic_sensor_driver_t *gsd)
{
    getopt_add_description (gsd->gopt, "Ocean-Server Remote Helm I/O driver.");
    return 0;
}


int
main (int argc, char *argv[])
{
    state_t *state = calloc (1, sizeof (state_t));
    os_conduit_init (&state->os_conduit);

    fasttrig_init ();

    state->gsd = gsd_create (argc, argv, "remote-helm.os-conduit", &myopts);
    gsd_canonical (state->gsd, '\r','\n');
    gsd_launch (state->gsd);

    senlcm_raw_t_subscribe (state->gsd->lcm, "GPSD_CLIENT.RAW", &gpsd_client_callback, state);
    senlcm_dstar_ssp1_t_subscribe (state->gsd->lcm, "DESERT_STAR", &desert_star_callback, state);
    senlcm_rdi_pd4_t_subscribe (state->gsd->lcm, "RDI", &rdi_pd4_callback, state);
    if (USE_MICROSTRAIN_RPH)
        senlcm_ms_gx1_t_subscribe (state->gsd->lcm, "MICROSTRAIN", &microstrain_callback, state);
    else
        senlcm_os_compass_t_subscribe (state->gsd->lcm, "OS_COMPASS", &os_compass_callback, state);

    pthread_t tid;
    pthread_create (&tid, NULL, lcm_thread, state->gsd);
    pthread_detach (tid);

    gsd_flush (state->gsd);
    gsd_reset_stats (state->gsd);
    while (!state->gsd->done) {
        // read stream
        char buf[1024];
        int64_t timestamp;
        int len = gsd_read (state->gsd, buf, 128, &timestamp);
        if (len > 0)
            gsd_update_stats (state->gsd, 1);
    } // while
}
