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

#include "libcommon/ascii.h"
#include "libcommon/depth.h"
#include "libcommon/nmea.h"
#include "libcommon/units.h"
#include "libcommon/generic_sensor_driver.h"
#include "lcmtypes/senlcm_os_compass_t.h"

#include "os5000.h"

static char * DEFAULT_LOGDIR = "/home/administrator/"; 

static int
parse_buf (const char *buf, int len, senlcm_os_compass_t *osc)
{
    os5000_t os = os5000_parse_0x01 (buf, len);

    // rph in rad
    if (os.bitmask & OS5000_BITMASK_R)
        osc->rph[0] = os.R*DTOR;
    else
        return 0;
    if (os.bitmask & OS5000_BITMASK_P)
        osc->rph[1] = os.P*DTOR;
    else
        return 0;
    if (os.bitmask & OS5000_BITMASK_H)
        osc->rph[2] = os.H*DTOR;
    else
        return 0;

    // temperature in celsius
    if (os.bitmask & OS5000_BITMASK_T)
        osc->T = os.T;
    else
        return 0;

    // pressure in pascals
    if (os.bitmask & OS5000_BITMASK_D)
        osc->p_volts = os.D;
    else
        return 0;
    osc->p_meas = os5000_volts_to_psi (os.D)*UNITS_PSI_TO_PASCAL;
    static int nsamples = 0;
    static double p_o = 0;
    if (nsamples++ < 10)
        p_o = ((nsamples-1.0)/nsamples)*p_o + (1.0/nsamples)*osc->p_meas; // running average
    osc->p_o = p_o;
    osc->p_gage = osc->p_meas - osc->p_o;

    // depth in meters
    osc->depth = depth_dfs_simple (osc->p_gage);

    // magnetic field in Gauss
    if (os.bitmask & OS5000_BITMASK_M_XYZ) {
        osc->Mxyz[0] = os.Mxyz[0] * UNITS_MILLI_TO_ONE;
        osc->Mxyz[1] = os.Mxyz[1] * UNITS_MILLI_TO_ONE;
        osc->Mxyz[2] = os.Mxyz[2] * UNITS_MILLI_TO_ONE;
    }
    else
        return 0;

    // acceleration in G's
    if (os.bitmask & OS5000_BITMASK_A_XYZ) {
        osc->Gxyz[0] = os.Axyz[0];
        osc->Gxyz[1] = os.Axyz[1];
        osc->Gxyz[2] = os.Axyz[2];
    }
    else
        return 0;

    return 1;
}

static int
os_config (generic_sensor_driver_t *gsd)
{
#define SEND_TO_COMPASS() gsd_write (gsd, buf, len); usleep (250e3)

    int len = 0;
    char buf[2048];

    printf ("configuring OS5000 for Iver use... ");

    /* raw volts output for depth */
    len = sprintf (buf, "%cM\r", ASCII_ESC);
    SEND_TO_COMPASS();
    len = sprintf (buf, "0\r");
    SEND_TO_COMPASS();

    /* select desired fields */
    const int desired = 
        OS5000_BITMASK_H + OS5000_BITMASK_P + OS5000_BITMASK_R +
        OS5000_BITMASK_T + OS5000_BITMASK_D + OS5000_BITMASK_M_XYZ + OS5000_BITMASK_A_XYZ;
    len = sprintf (buf, "%cX\r", ASCII_ESC);
    SEND_TO_COMPASS();
    len = sprintf (buf, "%d\r", desired);
    SEND_TO_COMPASS();

    /* select desired output format */
    len = sprintf (buf, "%c*\r", ASCII_ESC);
    SEND_TO_COMPASS();
    len = sprintf (buf, "1\r");
    SEND_TO_COMPASS();

    printf ("ready!\n");

    return 1;
}


static int
myopts (generic_sensor_driver_t *gsd)
{
    getopt_add_description (gsd->gopt, "Ocean-Server OS5500-USG digital compass sensor driver.");
    return 0;
}

FILE* create_compass_file(char *logdir){
  char buffer_path[256];
  char buffer_file[256];
  time_t curtime;
  struct tm *loctime;

  //get the current time
  curtime = time (NULL);

  //convert it to local time
  loctime = localtime (&curtime);

  //print to a string
  strftime (buffer_file,
	    256,
	    "/compass_%d%b%y_%H%M%S.dat",
	    loctime);
  strcpy(buffer_path,logdir);
  strcat(buffer_path,buffer_file);
    
  printf("logging file: %s\n", buffer_path);

  FILE* fid = fopen(strdup(buffer_path), "w");
  fprintf (fid, "OS-5000 compass output from CollieCam\n");
  strftime (buffer_path,
            256,
            "%d-%b-%y %H:%M.%S\n",
            loctime);
  fprintf (fid, "File started on %s\n", buffer_path);
  fprintf (fid, "Utime\t Roll\t Pitch\t Head\t T\t Depth\t P_volts\t P_meas\t P_gage\t P_o\t Mx\t My\t Mz\t Gx\t Gy\t Gz\n");

  return fid;
}

void
compass_out_to_file (FILE* fid, senlcm_os_compass_t os_compass){
	    fprintf(fid, "%lld\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\t %lf\n",
		    os_compass.utime,
		    os_compass.rph[1],
		    os_compass.rph[2],
		    os_compass.rph[3],
		    os_compass.T,  
		    os_compass.depth,
		    os_compass.p_volts,
		    os_compass.p_meas,
		    os_compass.p_gage,
		    os_compass.p_o,
		    os_compass.Mxyz[1],
		    os_compass.Mxyz[2],
		    os_compass.Mxyz[3],
		    os_compass.Gxyz[1],
                    os_compass.Gxyz[2],
                    os_compass.Gxyz[3]);

}

/*void
sync_callback (const lcmrecv_buf_t *rbuf, const char *channel,
	       const botlcm_image_sync_t *sync, void * user)
{
generic_sensor_driver_t *gsd = */

static void *
compass_thread (void *context)
{
  generic_sensor_driver_t *gsd = context;

  while (1) {
    char buf[1024];
    int64_t timestamp;
    int len = gsd_read (gsd, buf, 128, &timestamp);
    senlcm_os_compass_t os_compass;
    if (parse_buf (buf, len, &os_compass)) {
      os_compass.utime = timestamp;
      senlcm_os_compass_t_publish (gsd->lcm, gsd->channel, &os_compass);
      gsd_update_stats (gsd, 1);

      compass_out_to_file (gsd->fid, os_compass);
    }
    else
      gsd_update_stats (gsd, -1);

   }// while                              

  return 0;
}
int
main (int argc, char *argv[])
{

    generic_sensor_driver_t *gsd = gsd_create (argc, argv, NULL, myopts);
    gsd->fid = create_compass_file(gsd->logdir); 
    gsd_canonical (gsd, '\r','\n');
    gsd_launch (gsd);

    os_config (gsd);
    
    gsd_flush (gsd);
    gsd_reset_stats (gsd);

    gsd->lcm = lcm_create (NULL);

    //    botlcm_image_sync_t_subscribe (gsd->lcm, 
    //				  "PROSILICA_M.SYNC", 
    //				  &sync_callback, gsd)   

    pthread_t tid;
    pthread_create (&tid, NULL, compass_thread, gsd);
    pthread_detach (tid);

    while(1)
      lcm_handle(gsd->lcm);
    
    fclose(gsd->fid);

    return 0;
}
