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

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

#include "lcmtypes/senlcm_rdi_pd4_t.h"
#include "lcmtypes/senlcm_rdi_pd5_t.h"

#include "rdi.h"

#define RDI_DEV_TICKS_PER_SECOND (100) //  1/tofp_hundredth
#define RDI_DEV_TICKS_WRAPAROUND (24*3600*100)

static rdi_pd_mode_t rdi_pd_mode = RDI_PD4_MODE;
static int rdi_pd_len = RDI_PD4_LEN;

static int64_t
rdi_timestamp_sync (timestamp_sync_state_t *tss, int64_t tofp_hours, int64_t tofp_minute,
                    int64_t tofp_second, int64_t tofp_hundredth, int64_t host_utime)
{
    int64_t dev_ticks = (tofp_hours*3600 + tofp_minute*60 + tofp_second)*100 + tofp_hundredth;
    
    return timestamp_sync (tss, dev_ticks, host_utime);
}

static void
rdi_pd4_to_lcm_pd4 (const rdi_pd4_t *pd4, senlcm_rdi_pd4_t *lcm_pd4)
{
    lcm_pd4->system_config = pd4->system_config;

    int nbeams = 0;
    double altsum = 0;
    for (int i=0; i<4; i++) {
        lcm_pd4->btv[i] = pd4->btv[i] * UNITS_MILLI_TO_ONE; // mm/s to m/s
        lcm_pd4->wtv[i] = pd4->wtv[i] * UNITS_MILLI_TO_ONE; // mm/s to m/s

        if (pd4->altitude[i] > 0) {
            altsum += pd4->altitude[i] * UNITS_CENTI_TO_ONE; // cm to m
            nbeams++;
            const double cos30 = 0.866025403784439; // cos(30*DTOR)
            lcm_pd4->range[i] = pd4->altitude[i] * UNITS_CENTI_TO_ONE / cos30; // cm to m
        }
        else
            lcm_pd4->range[i] = -1;
    }
    if (nbeams > 0)
        lcm_pd4->altitude = altsum/nbeams; // avg altitude [m]
    else
        lcm_pd4->altitude = -1;

    lcm_pd4->btv_status      = pd4->btv_status;
    lcm_pd4->wtv_status      = pd4->wtv_status;
    lcm_pd4->wtv_layer_start = pd4->wtv_layer_start * UNITS_DECI_TO_ONE; // dm to m
    lcm_pd4->wtv_layer_end   = pd4->wtv_layer_end * UNITS_DECI_TO_ONE;   // dm to m

    lcm_pd4->tofp_hour      = pd4->tofp_hour;
    lcm_pd4->tofp_minute    = pd4->tofp_minute;
    lcm_pd4->tofp_second    = pd4->tofp_second;
    lcm_pd4->tofp_hundredth = pd4->tofp_hundredth;

    memcpy(&lcm_pd4->builtin_test, &pd4->builtin_test, sizeof (pd4->builtin_test));

    lcm_pd4->speed_of_sound   = pd4->speed_of_sound;       // m/s
    lcm_pd4->xducer_head_temp = pd4->xducer_head_temp * UNITS_CENTI_TO_ONE; // cC to C
}

static void
rdi_pd5_to_lcm_pd5 (const rdi_pd5_t *pd5, senlcm_rdi_pd5_t *lcm_pd5)
{
    rdi_pd4_to_lcm_pd4 ( (rdi_pd4_t *) pd5, &lcm_pd5->pd4);

    lcm_pd5->salinity = pd5->salinity; // ppt
    lcm_pd5->depth    = pd5->depth * 0.1; // dm to m
    
    lcm_pd5->pitch   = pd5->pitch * UNITS_CENTI_TO_ONE * DTOR;   // .01 deg to rad
    lcm_pd5->roll    = pd5->roll * UNITS_CENTI_TO_ONE * DTOR;    // .01 deg to rad
    lcm_pd5->heading = pd5->heading * UNITS_CENTI_TO_ONE * DTOR; // .01 deg to rad

    for (int i=0; i<4; i++) {
        lcm_pd5->dmg_btv[i] = pd5->dmg_btv[i] * UNITS_DECI_TO_ONE; // dm to m
        lcm_pd5->dmg_wtv[i] = pd5->dmg_wtv[i] * UNITS_DECI_TO_ONE; // dm to m
    }
}

static void
program_dvl (generic_sensor_driver_t *gsd, const char *config)
{
    char dvlconfig[1024];
    sprintf (dvlconfig, "%s/%s", gsd->rootdir, config);

    if (gsd->io == GSD_IO_PLAYBACK)
        return;

    printf ("Initializing DVL according to %s\n", dvlconfig);
    FILE *fptr = fopen (dvlconfig, "r");
    if (fptr == NULL) {
        PERROR ("fopen()");
        exit (EXIT_FAILURE);
    }

    /* block until we know dvl is alive */
    char buf[256];
    bool alive=0;
    while (!alive) {
        gsd_write (gsd, "===\n", strlen ("===\n")); // software break
        alive = gsd_read_timeout (gsd, buf, 256, NULL, 100E3);
    }
    printf ("\n");

    char line[256];
    while (gsd_read_timeout (gsd, line, 256, NULL, 100E3)>0) {
        printf ("%s", line);
    }

    printf ("--------------------programming---------------------------------\n");
    while (fgets (line, 256, fptr) != NULL) {
        if (line[0] == ';') continue; // comment

        printf ("%s", line);
        gsd_write (gsd, line, strlen (line));

        if (!strncmp (line, "#PD4", 4)) {
            rdi_pd_mode = RDI_PD4_MODE;
            rdi_pd_len  = RDI_PD4_LEN;
        }
        if (!strncmp (line, "#PD5", 4)) {
            rdi_pd_mode = RDI_PD5_MODE;
            rdi_pd_len  = RDI_PD5_LEN;
        }
        usleep (100E3); // need to give DVL some time for command to take
    } // while
    fclose (fptr);
    printf ("--------------------done-----------------------------------------\n");
}


static int
myopts (generic_sensor_driver_t *gsd)
{
    getopt_add_description (gsd->gopt, "RDI Workhorse & Explorer DVL sensor driver.");
    getopt_add_string (gsd->gopt, '\0', "config", "", "RDI initialization configuration script.");
    return 0;
}


int
main (int argc, char *argv[])
{
    timestamp_sync_state_t *tss = timestamp_sync_init (RDI_DEV_TICKS_PER_SECOND, RDI_DEV_TICKS_WRAPAROUND, 1.01);

    generic_sensor_driver_t *gsd = gsd_create (argc, argv, NULL, &myopts);
    gsd_launch (gsd);

    // initialize dvl
    if (getopt_has_flag (gsd->gopt, "config"))
        program_dvl (gsd, getopt_get_string (gsd->gopt, "config"));
    else {
        char *config;
        char key[256];
        sprintf (key, "%s.%s", gsd->rootkey, "config");
        if (0==config_get_string (gsd->cfg, key, &config))
            program_dvl (gsd, config);
    }
    gsd_noncanonical (gsd, rdi_pd_len, 1);

    gsd_flush (gsd);
    gsd_reset_stats (gsd);
    while (1) {
        // read stream
        char buf[1024];
        int64_t timestamp;
        int len = gsd_read (gsd, buf, rdi_pd_len, &timestamp);

        // try to parse it
        switch (rdi_pd_mode) {   
        case RDI_PD4_MODE: {
            rdi_pd4_t pd4;
            int ret = rdi_parse_pd4 (buf, len, &pd4);
            if (ret > 0) {
                senlcm_rdi_pd4_t lcm_pd4;
                rdi_pd4_to_lcm_pd4 (&pd4, &lcm_pd4);
                lcm_pd4.utime = rdi_timestamp_sync (tss, pd4.tofp_hour, pd4.tofp_minute, pd4.tofp_second, 
                                                    pd4.tofp_hundredth, timestamp);
                senlcm_rdi_pd4_t_publish (gsd->lcm, gsd->channel, &lcm_pd4);
            }
            gsd_update_stats (gsd, ret);
            break;
        }
        case RDI_PD5_MODE: {
            rdi_pd5_t pd5;
            int ret = rdi_parse_pd5 (buf, len, &pd5);
            if (ret > 0) {
                senlcm_rdi_pd5_t lcm_pd5;
                rdi_pd5_to_lcm_pd5 (&pd5, &lcm_pd5);
                lcm_pd5.utime = lcm_pd5.pd4.utime = rdi_timestamp_sync (tss, pd5.tofp_hour, pd5.tofp_minute, 
                                                                        pd5.tofp_second, pd5.tofp_hundredth, timestamp);
                senlcm_rdi_pd5_t_publish (gsd->lcm, gsd->channel, &lcm_pd5);
            }
            gsd_update_stats (gsd, ret);
            break;
        }
        default:
            ERROR ("unsupported PD mode by driver");
            gsd_update_stats (gsd, -1);
        } // switch

    } // while

    timestamp_sync_free (tss);
}
