/*****************************************************************************

   FILE  : nmea_gps.c
   USAGE : nmea_gps <control file name>
   INPUT : GPS NMEA formatted data file
   OUTPUT: 1) simple ASCII GPS file, if requested
           2) Matlab data formatted file, if requested

   This program reads in a NMEA GPS data file and writes the information
   to a tabular ASCII file, which can then be used with the codas3/adcp/nav
   programs for calculating absolute reference layer velocities, etc.

   If the user so requests, it can also generate Matlab .mat files with
   the same information edited according to the criteria specified in
   the control file.

   The control file has the following structure:

   ---------------------------------------------------------------------

    YMD_BASE:           <yyyy/mm/dd>
    TIME_RANGE:         <ALL | yyyy/mm/dd hh:mm:ss to yyyy/mm/dd hh:mm:ss>
    TIME_FILE:          <decimal day time file>
    MESSAGE:            <GPGGA | LGGGA>
    REQUIRE_CHECKSUM
    OUTPUT_ASCII_FILE:  <none | output ASCII file>
    OUTPUT_MAT_FILE:    <none | matlab data file>
        max_HDOP=       <d>
        subsamples=     <n>
        max_mat_array_size=  <n>
        SAVE_NUM_OF_SAT
        SAVE_HDOP
        SAVE_ALTITUDE
        GAP_INCLUDED
      end

    INPUT_GPS_FILES:
       <NMEA formatted GPS files>
        .
        .
        .
  -------------------------------------------------------------------------

   All input parameters are optional except 'INPUT_GPS_FILES:'.

   Following is a brief explanation of each parameter input.

   (1) YMD_BASE:
        specifies the year-month-day base in the form of yyyy/mm/dd.
        Default:  00/01/01

        Since dates are not recorded in the NMEA GPS formatted files, the
        year-month-day base will be used for decimal day calculation.
        Although technically optional, this parameter should
        always be included.

   (2) TIME_RANGE:
        specifies the time range to be retrieved, in the form

             yyyy/mm/dd hh:mm:ss  to  yyyy/mm/dd hh:mm:ss

        or 'ALL' to retrieve all data from the input files.
        Default:  ALL

   (3) TIME_FILE:
        gives the name of a file, such as an adcpsect .nav
        file, with the times for which fixes are desired.
        The time is the first field on each non-commented
        (%) line, interpreted as a decimal day.  The
        remainder of the line is ignored.
        The first fix after the time in the file is the one
        that is output.
        Default: none.

   (4) OUTPUT_ASCII_FILE:
        specifies name (root) of output ASCII gps file, or 'none' for
        no ASCII output. The extension of the file name will be '.gps'.
        Default: none.

        The format of ASCII gps file will be:

         [time  Longitude Latitude nsat quality hdop ndop edop vdop altitude]

       In NMEA gps formatted data, there is no information on
       ndop, edop and vdop.  These columns will display only 0s.
       (This format with columns of zeros is a historical
       artifact from the days of the MX1100 series of
       Transit and Transit/GPS receivers.)

   (5) MESSAGE:  <GPGGA | LGGGA | ??GGA>
        specifies the NMEA message type.  GPGGA is most
        common; the Trimble 10X puts out LGGGA.  The missing
        height field in the LGGGA message is set to 0.  Other
        "GGA" type messages ($??GGA, where the ?? denote any
        characters) are allowed with the ??GGA option.  This
        most permissive option is now the default.

   (6) REQUIRE_CHECKSUM
        If this keyword is included, the NMEA-standard
        checksum calculation will be performed on each input
        line, and only lines with correct checksums will be
        used.  This is highly recommended if the input
        stream includes checksums; some GPS receivers use
        them, some don't.

   (7) OUTPUT_MAT_FILE:    <none | matlab data file>
        specifies name (root) of MATLAB data output file, or 'none' for
        no MATLAB data output. The extension of the file name will be
        '.mat'.
        Default: none.

        The basic variables will be saved in matlab file:
               [time longitude latitude].

  Following parameters are only applied with OUTPUT_MAT_FILE selected.

        (a) max_HDOP=
            Specifies the maximum hdop value that will be accepted for
            a record to output to matlab data file.
            Default:  accept all.

        (b) subsamples=
            Specifies the number of data record will be selected from
            every 'subsample' record(s).
            Default:  1  (every record will be saved)

        (c) max_mat_array_size=
            Specifies the maximum array size will be limited for each
            matlab data file.  Normally, used for PC matlab data file
            (8,000 is suggested).  For 386-matlab and SUN there are no
            limitation of the data array size.   If the output data can
            not be fitted in one matlab data file, then the next matlab
            data file will be created with file name extention '.m01',
            '.m02', ... and so on.
            Default:  no limitation of the array size.

        (d) SAVE_NUM_OF_SAT
            Store the number of satellites used in the fix.
            Without this keyword, nsat will not be saved.

        (e) SAVE_HDOP
            Store the horizontal dilution of precision.
            Without this keyword, hdop will not be saved.

        (f) SAVE_ALTITUDE
            Store the altitude.  Without this keyword,
            altitude will not be saved.

        (g) GAP_INCLUDED
            Store a record of NaNs as a place-holder if a
            valid sample is not available, that is, if
            if quality = 0 or hdop > max_hdop.  Without this
            keyword, nothing is output in this case.

   (7) end
       keyword required after all options above, and before
       the following keyword:

   (8) INPUT_GPS_FILES:
       following this keyword, input NMEA gps file(s) must be provided
       in chronological order.  During the conversion, we assume that
       there are no gaps of one day or longer.
-----------------------------------------------------------------------------
         Willa Zhu        09/27/91

    Wed  95/05/31 E.F.
         Added support for LGGGA message format.

    Mon  95/06/12 E.F.
         Added the TIME_FILE: option.

    95/06/21 E.F.
         Bug fix and cleanup, so that too-short message lines are
         handled properly.  Previously, a short message line acted
         like an EOF.

    Tue  97/07/15 E.F.
         Added SAVE_ALTITUDE and REQUIRE_CHECKSUM options.

    2000/02/22 EF
         Added ??GGA option, and relaxed requirement for message
         to contain nsat and quality fields.  Now the message only
         needs to be complete through the position.

    2000/09/16 EF
         Changed to reject fix with time equal to or earlier than a
         previous fix, and to display a warning.

---------------------------------------------------------------------------- */

#include "common.h"
#include "time_.h"
#include "ioserv.h"
#include "matfile.h"
#include "use_db.h"

#ifdef VBUF
#undef VBUF
#endif

#define VBUF 1         /* TRUE is OK only for Turbo-C and other
                          compilers that automatically allocate
                          space for a buffer if the buffer pointer
                          in the argument list of setvbuf() is null.
                       */
#define VBUFSIZE 8192

typedef struct{
   int degrees, minutes;
   float fraction;
   char hemisphere;
} DMQ_LL_TYPE;

/* In the above, it is easier to use a float for the
   fraction because the number of digits of precision is not
   fixed, but can be set from the keyboard of the 5400.  By
   reading this fractional part as a float, we avoid the need
   to count the digits.
*/

typedef struct{
   double hdop;
   int hours, minutes, seconds;
   DMQ_LL_TYPE lat, lon;
   int quality, nsat;
   double height;
} GGA_TYPE;

typedef struct{
   double ddtime;
   double lon, lat;
} FIX_L_TYPE;

typedef struct{
   GGA_TYPE gga;
   FIX_L_TYPE dec;  /* decimal (double precision) */
} FIX_TYPE;


#if PROTOTYPE_ALLOWED
int  GetTimeRange(FILE *fpcnt);
void WriteMat(FIX_TYPE *fix);
void CloseMat(void);
void WriteAscii(FIX_TYPE *fix);
int  readRecord(FIX_TYPE *fix);
int  readTime(double *time_ptr);
#else
int  GetTimeRange();
void WriteMat();
void CloseMat();
void WriteAscii();
int  readRecord();
int  readTime();
#endif


static FILE   *fp_in, *fp_out, *fp_mat, *fp_time;
static char   in_file[180], out_file[176], mat_file[176], ymd_base[40],
              OutFile[180], MatFile[180], time_file[180],
              message_name[40];
static int    message_type;
static int    SaveNsat=0, SaveHdop=0, GapInc=0, SaveAlt=0, subsample=1, Check=0;
static int    asciiflag= 0, matflag=0, timeflag = 0, finished;
static int    AllFile=1, totalDay = 0, baseDay=0;
static long   filePos;
static double startDay, endDay, NewHour, OldHour=0;
YMDHMS_TIME_TYPE base, startT, endT, time;
FIX_TYPE    old_fix, new_fix;

MAT_ARRAY_FP_TYPE *gps_mat_ptr, *nsat_mat_ptr, *hdop_mat_ptr;
static int    nrow=0, imax, count, numMatfile, matclosed, MAX_MAT_SIZE=0;
static unsigned int nrecord=0;
static double max_hdop=MAXDOUBLE;
static int    status;
int jt=1, jlon=2, jlat=3, jnsat, jhdop, jalt;

static double ens_time;

#define GPGGA 1
#define LGGGA 2
#define GGA 3
static NAME_LIST_ENTRY_TYPE message_names[] =
{
   {"GPGGA", GPGGA},
   {"LGGGA", LGGGA},
   {"??GGA", GGA},
   {NULL, 0}
};

static OPTION_TYPE options[] =
{
   {"end"               , 0          , NULL         , NULL               },
   {"YMD_BASE:"         , TYPE_STRING, op_get_param , ymd_base           },
   {"TIME_RANGE:"       , 0          , GetTimeRange , (char *) NULL      },
   {"OUTPUT_ASCII_FILE:", TYPE_STRING, op_get_param , out_file           },
   {"OUTPUT_MAT_FILE:"  , TYPE_STRING, op_get_param , mat_file           },
   {"TIME_FILE:"        , TYPE_STRING, op_get_param , time_file          },
   {"MESSAGE:"          , TYPE_STRING, op_get_param , message_name        },
   {"max_HDOP="         , TYPE_DOUBLE, op_get_param , (char *) &max_hdop },
   {"subsamples="       , TYPE_INT   , op_get_param , (char *) &subsample},
   {"max_mat_array_size=", TYPE_INT  , op_get_param , (char *) &MAX_MAT_SIZE},
   {"SAVE_HDOP"         , 1          , set_code     , (char *) &SaveHdop },
   {"SAVE_NUM_OF_SAT"   , 1          , set_code     , (char *) &SaveNsat },
   {"SAVE_ALTITUDE"     , 1          , set_code     , (char *) &SaveAlt  },
   {"GAP_INCLUDED"      , 1          , set_code     , (char *) &GapInc   },
   {"REQUIRE_CHECKSUM"  , 1          , set_code     , (char *) &Check    },
   { NULL               , 0          , NULL         , NULL               }
};

static PARAMETER_LIST_ENTRY_TYPE parameters[] =
{
   { "INPUT_GPS_FILES:" ,  NULL  , NULL, 0           },
   { NULL               ,  NULL  , NULL, 0           }
};                   /* parameters[] */

#if PROTOTYPE_ALLOWED
void do_it(FILE *fpcnt)
#else
void do_it(fpcnt)
FILE *fpcnt;
#endif
{
/*--------------------- set defaults on options -----------------------*/

   strcpy(ymd_base, "");
   strcpy(out_file, "");
   strcpy(mat_file, "");
   strcpy(time_file, "");
   strcpy(message_name, "??GGA");

/*------------------- read user-specified options ---------------------*/

   if (!execute_options(fpcnt, options, ECHO)) return;
   if (get_parameters(fpcnt, parameters, NULL))  exit(-1);

   message_type = get_code(message_names, message_name);
   check_error( message_type == 0 , "invalid MESSAGE: ");
   if (strcmp(ymd_base, ""))
   {
      if(sscanf(ymd_base,"%hd/%2hd/%2hd",&base.year,&base.month,&base.day) != 3)
      {
         printf("ERROR: input parameter YMD_BASE.\n"); exit(-1);
      }
      else
      {
         base.year = yr4digit(base.year);
         base.hour = 0;
         base.minute = 0;
         base.second = 0;
         baseDay = year_day(&base,base.year);
      }
   }

/* checking I/O files */

   if (!(strcmp(out_file, "") == 0 || strcmp(out_file,"none") == 0))
   {
      new_extension(OutFile, out_file, ".gps");
      fp_out = check_fopen(OutFile, "w");
      asciiflag = 1;
#ifdef VBUF
      check_error(setvbuf(fp_out,  NULL, _IOFBF, VBUFSIZE),
                 "Error in setvbuf in main.");
#endif
   }

   if (!(strcmp(time_file, "") == 0))
   {
      fp_time = check_fopen(time_file, "r");
      timeflag = 1;
#ifdef VBUF
      check_error(setvbuf(fp_time,  NULL, _IOFBF, VBUFSIZE),
                 "Error in setvbuf in main.");
#endif
   }


   if (!(strcmp(mat_file, "") == 0 || strcmp(mat_file,"none") == 0))
   {
      new_extension(MatFile, mat_file, ".mat");
      fp_mat   = check_fopen(MatFile, "wb");
#ifdef VBUF
      check_error(setvbuf(fp_mat,  NULL, _IOFBF, VBUFSIZE),
                 "Error in setvbuf in main.");
#endif
      matflag = 1;
      nrow = 3;
      if(SaveNsat) { nrow++; jnsat = nrow;}
      if(SaveHdop) { nrow++; jhdop = nrow;}
      if(SaveAlt)  { nrow++; jalt  = nrow;}
      gps_mat_ptr = start_mat_array(fp_mat, 'd', "gps", 'c', nrow);
      imax = MAX_MAT_SIZE / nrow;
      nrecord = 0;
      count = 0;
      matclosed = 0;
      numMatfile = 1;
   }

   if(asciiflag == 0 && matflag == 0)
   {
      printf("\n Neither ascii nor mat output file is specified.\n");
      exit(-1);
   }

   filePos = ftell(fpcnt);
   while (fscanf(fpcnt,"%s",in_file) == 1)
   {
      fp_in = check_fopen(in_file, "r");
      fclose(fp_in);
   }
   fseek(fpcnt,filePos,0);

   if (!AllFile)
   {
      startDay = year_day(&startT,base.year);
      endDay = year_day(&endT,base.year);
   }

   printf("INPUT_GPS_FILES:\n");
   ens_time = -BADFLOAT;
   finished = 0;
   while (!(finished || fscanf(fpcnt,"%s",in_file) != 1))
   {
      printf("          %s\n",in_file);
      fp_in = check_fopen(in_file, "r");
#ifdef VBUF
      check_error(setvbuf(fp_in,  NULL, _IOFBF, VBUFSIZE),
                 "Error in setvbuf in main.");
#endif
      if(!AllFile)      /* find the beginning of time to be output */
      {
         while(((status = readRecord(&new_fix)) != EOF) &&
              ((baseDay+totalDay+NewHour) < startDay))
            ;
      }
      else
      {
         status = readRecord(&new_fix);
      }

      if (timeflag && ens_time == -BADFLOAT)
      {
         old_fix = new_fix;
         while (status != EOF && ens_time <= new_fix.dec.ddtime)
         {
            status = readTime(&ens_time);
         }
      }

      while(status != EOF && !finished)
      {
         if (old_fix.dec.ddtime < new_fix.dec.ddtime)
         {
            if (!timeflag || (old_fix.dec.ddtime < ens_time &&
                              new_fix.dec.ddtime >= ens_time) )
            {
               if(asciiflag) WriteAscii(&new_fix);
               if(matflag)   WriteMat(&new_fix);
               while (timeflag && status != EOF && ens_time <= new_fix.dec.ddtime)
                  status = readTime(&ens_time);
            }
            old_fix = new_fix;
         }
         else
         {
            printf("Warning: ignoring fix time %12.7lf, <= previous %12.7lf\n",
                     new_fix.dec.ddtime, old_fix.dec.ddtime);
         }
         if (timeflag && status == EOF)
            finished = 1;   /* quit: end of time list file */

         if (status != EOF)
            status = readRecord(&new_fix);

         if(status != EOF && !AllFile
                          && (baseDay+totalDay+NewHour) >= endDay)
            finished = 1;   /* quit: end of specified time range */
      }
      fclose(fp_in);
   }

   if(asciiflag) fclose(fp_out);
   if(timeflag)  fclose(fp_time);

   if(matflag && !matclosed)
   {
      CloseMat();
   }
}  /* do_it */

#if PROTOTYPE_ALLOWED
void CloseMat(void)
#else
void CloseMat()
#endif
{
      end_mat_array(gps_mat_ptr);
      savemat(fp_mat, 'i', "j_time",1, 1, 0, (char *)&jt, (char *)NULL);
      savemat(fp_mat, 'i', "j_lon",1, 1, 0, (char *)&jlon, (char *)NULL);
      savemat(fp_mat, 'i', "j_lat",1, 1, 0, (char *)&jlat, (char *)NULL);
      if(SaveNsat)
        savemat(fp_mat, 'i', "j_nsat",1, 1, 0, (char *)&jnsat, (char *)NULL);
      if(SaveHdop)
        savemat(fp_mat, 'i', "j_hdop",1, 1, 0, (char *)&jhdop, (char *)NULL);
      if(SaveAlt)
        savemat(fp_mat, 'i', "j_alt",1, 1, 0, (char *)&jalt, (char *)NULL);
      fclose(fp_mat);
      printf("\n Mat file Size:  %d x %u\n Number of Mat file: %d\n ",
                                        nrow, nrecord, numMatfile);
      matclosed = 1;
}

#if PROTOTYPE_ALLOWED
int readTime(double *time_ptr)
#else
int readTime(time_ptr)
double *time_ptr;
#endif
{
   static char buf[128];
   int n = 0, found = 0;

   while (!found && n != EOF)
   {
      n = getline_nc(fp_time, buf, 128);
      found = sscanf(buf, "%lf", time_ptr);
   }
   if (found == 1)
   {
      return 0;
   }
   return EOF;
}


#if PROTOTYPE_ALLOWED
int GetTimeRange(FILE *fpcnt)
#else
int GetTimeRange(fpcnt)
FILE *fpcnt;
#endif
{
  char  str[80];

  filePos = ftell(fpcnt);
  if ((fscanf(fpcnt," %s",str) != 1)) exit(-1);
  else
    if(!(strcmp(str,"ALL") == 0 || strcmp(str,"all") == 0)) {
      AllFile = 0;
      fseek(fpcnt,filePos,0);
      if(get_time_range(fpcnt, &startT, &endT) != 1) exit(-1);
      printf("   %d/%02d/%02d %02d:%02d:%02d to %d/%02d/%02d %02d:%02d:%02d\n",
               startT.year,startT.month,startT.day,startT.hour,startT.minute,
               startT.second,endT.year,endT.month,endT.day,endT.hour,
               endT.minute,endT.second);
    }
    else printf(" %s\n",str);
  return(0);
}

#if PROTOTYPE_ALLOWED
int GoodNMEA(char *buf)
#else
int GoodNMEA(buf)
char *buf;
#endif
{
   extern int Check;             /* Reminder, in case we use this routine
                                    elsewhere.
                                 */
   int len, i, ofs, nscan, msg_chk;
   unsigned char chk = 0;

   len = strlen(buf);
   if (len < 30 || buf[0] != '$')    return(0);
   /* hard coded 30 is not general for NMEA,
                                    but specific to GGA; change it if this
                                    routine is used elsewhere for more generic
                                    NMEA checking.
                                  */
   if (!Check)      return(1);
   while (buf[len-1] == '\n' || buf[len-1] == '\r')
   {
      len--;
      buf[len] = NUL;
   }
   ofs = len -3;
   nscan = sscanf(buf+ofs, "*%2x", &msg_chk);
   if (nscan != 1)      return(0);
   for (i = 1; i < len - 3; i++)
   {
      chk ^= buf[i];
   }
   /* printf("%x  %x\n", chk, msg_chk); */
   if (chk == msg_chk)
      return(1);
   return(0);

}


#if PROTOTYPE_ALLOWED
int readRecord(FIX_TYPE *fix)
#else
int readRecord(fix)
FIX_TYPE *fix;
#endif
{
   char  line_buffer[120], id[10];
   int   buf_len, nr, GotOne=0;
   double second;
   static GGA_TYPE gga;  /* local variable for notational convenience */

   while(!GotOne)
   {
      if ((buf_len = getline_nc(fp_in, line_buffer, 120)) == EOF)
      {
         return EOF;
      }
      else if (GoodNMEA(line_buffer))
      {
         sscanf(line_buffer, "%6s,",id);
         if( (message_type == GPGGA && strcmp(id,"$GPGGA") == 0 ) ||
             (message_type == LGGGA && strcmp(id,"$LGGGA") == 0 ) ||
             (message_type == GGA && id[0] == '$'
                                  && strcmp(id+3, "GGA") == 0 ) )
         {
            /* initialize  fields that are not required. */
            gga.quality = 9;
            gga.nsat = 0;
            gga.hdop = 0;
            gga.height = 0.0;
            nr = sscanf(line_buffer + 7,
               "%2hd%2hd%lf,%2d%2d%f,%c,%3d%2d%f,%c,%d,%d,%lf,%lf,",
               &(time.hour), &(time.minute), &second,&(gga.lat.degrees),
               &(gga.lat.minutes), &(gga.lat.fraction), &(gga.lat.hemisphere),
               &(gga.lon.degrees), &(gga.lon.minutes), &(gga.lon.fraction),
               &(gga.lon.hemisphere), &(gga.quality), &(gga.nsat), &(gga.hdop),
               &(gga.height));
            if( nr >= 11 )    /* through longitude at least */
            {
               GotOne = 1;
               time.second = (short int) second;
            }
         }
      }
   }
   time.year = 1900;
   time.month = 1;
   time.day = 1;
   NewHour = year_day(&time,time.year);
   if((NewHour - OldHour) < -1.0/24.0) totalDay++;
   OldHour = NewHour;
   fix->dec.lat = gga.lat.degrees + (gga.lat.minutes+gga.lat.fraction)/60;
   fix->dec.lon = gga.lon.degrees + (gga.lon.minutes+gga.lon.fraction)/60;
   if (gga.lat.hemisphere == 'S')  fix->dec.lat = -fix->dec.lat;
   if (gga.lon.hemisphere == 'W')  fix->dec.lon = -fix->dec.lon;
   fix->dec.ddtime =  baseDay + totalDay + NewHour;
   fix->gga = gga;
   return(0);
}


#if PROTOTYPE_ALLOWED
void WriteAscii(FIX_TYPE *fix)
#else
void WriteAscii(fix)
FIX_TYPE *fix;
#endif
{
    fprintf(fp_out, "%13.7f %13.7f %13.7f",
                        fix->dec.ddtime,fix->dec.lon,fix->dec.lat);
    fprintf(fp_out, "  %1hd %1hd %4.1f    0   0   0 %3.0f \n",
                       fix->gga.nsat,fix->gga.quality, fix->gga.hdop, fix->gga.height);
}

#if PROTOTYPE_ALLOWED
void WriteMat(FIX_TYPE *fix)
#else
void WriteMat(fix)
FIX_TYPE *fix;
#endif
{
   double d_holder[6];
   char ext[5];
   int i;

   if(matclosed) {
     sprintf(ext,".m%02d",numMatfile);
     new_extension(MatFile, mat_file, ext);
     fp_mat   = check_fopen(MatFile, "wb");
#ifdef VBUF
    check_error(setvbuf(fp_mat,  NULL, _IOFBF, VBUFSIZE),
                 "Error in setvbuf in WriteMat.");
#endif
     gps_mat_ptr = start_mat_array(fp_mat, 'd', "gps", 'c', nrow);
     numMatfile++;
     matclosed = 0;
     nrecord = 0;
     }

   if(count == 0) {
     if ((fix->gga.quality == 0 && GapInc) || (fix->gga.hdop > max_hdop)) {
        for (i=0; i<nrow; i++)   d_holder[i] = BADDOUBLE;
        tonan_d(d_holder, d_holder, nrow);
        }
     else if (fix->gga.quality > 0) {
        d_holder[0] = fix->dec.ddtime;
        d_holder[1] = fix->dec.lon;
        d_holder[2] = fix->dec.lat;
        i = 3;
        if(SaveNsat) {d_holder[i] = (double) fix->gga.nsat;    i++;  }
        if(SaveHdop) {d_holder[i] = (double) fix->gga.hdop;    i++;  }
        if(SaveAlt)  {d_holder[i] = (double) fix->gga.height;  i++;  }
        }
     add_to_mat_array(gps_mat_ptr, (char *)d_holder);
     nrecord++;
     }
   if(count == subsample-1) count = 0;
   else if(!(fix->gga.quality == 0 && !GapInc)) count++;

   if(imax && nrecord >= imax) {
      CloseMat();
      }
}

