/*
   Thu  07-27-1989  E.F.
         made track segment array part of data area to
            avoid possible stack overflow.

   Fri  07-13-1990  E.F.
         Fixed a bug in the logic of optimize_positions
         which caused erroneous position determination when
         the position fixes were more frequent than the ADCP
         ensembles.  Also, made a small change in the code
         to calculate the deviation of each fix from the dr;
         this change has no effect on the result, but may
         make the logic more clear.

*/

#include "common.h"
#include "dbext.h"     /* BADFLOAT, ADJ_BADFLOAT */
#include "vstat.h"
#include "pos_to_m.h"
#include "cal.h"
#include "smoothr.h"

typedef struct {
      int i0, i1;
   } SEGINDEX_TYPE;

static int segment_array_size;
static TRACK_SEGMENT_TYPE *segment;


extern double ensemble_time;   /* in seconds, ensemble interval */
extern double max_gap_distance, max_gap_time;

#if PROTOTYPE_ALLOWED
int is_gap(SMOOTHR_OUT_TYPE *sm, SMOOTHR_OUT_TYPE *prev_sm)
#else
int is_gap(sm, prev_sm)
SMOOTHR_OUT_TYPE *sm, *prev_sm;
#endif
{
   double dt, du, dv, dvel;
   extern double max_gap_distance, max_gap_time;

   /* We do not want a bad value to count as a gap; bad
      values are caught and handled separately.
   */
   if (prev_sm->su >= ADJ_BADFLOAT || sm->su >= ADJ_BADFLOAT)
      return (0);
   dt = (sm->t - prev_sm->t) * s_per_day - ensemble_time;
   du = sm->su - prev_sm->su;
   dv = sm->sv - prev_sm->sv;
   dvel = sqrt(du * du  +  dv * dv);
   return ((dt * dvel > max_gap_distance) || (dt > max_gap_time));
}

/* Note: the following function allocates the memory for "segment",
a pointer to the segment array.  Therefore it must not take "segment"
as an argument--if it did, realloc would change the local copy of
the pointer, not the global pointer itself.  The SAME applies to
the function that calls it, find_segments.
*/


#if PROTOTYPE_ALLOWED
void enlarge_segment_array(void)
#else
void enlarge_segment_array()
#endif
{
      size_t old_size = segment_array_size;
      segment_array_size += NSEGMENTS; 
      check_error( (segment_array_size <= old_size),
                    "In optpos.c, segment array exceeded max size for DOS");
      /* The following use of malloc is needed only for old compilers 
         such as SunOs 4.x; but it never hurts.
      */                         
      if (segment == NULL)
         segment = (TRACK_SEGMENT_TYPE *)malloc(
                  segment_array_size * sizeof(TRACK_SEGMENT_TYPE));
      else
         segment = (TRACK_SEGMENT_TYPE *)realloc(segment,
                  segment_array_size * sizeof(TRACK_SEGMENT_TYPE));
 
      check_error((segment == NULL), 
               "Allocation of memory for segment array failed in optpos.c");
}


#if PROTOTYPE_ALLOWED
int find_segments(FILE *fp_sm)
#else
int find_segments(fp_sm)
FILE *fp_sm;
#endif
{
   SMOOTHR_OUT_TYPE sm, prev_sm;
   int ism;
   int iseg;
   int nseg;
   int gap, last_gap;
   int bad, last_bad;
   int gap_boundary;

   segment_array_size = 0;
   segment = NULL; /* start with no memory allocated */

   ism = -1;
   iseg = 0;
   nseg = 0;
   last_bad = 1;
   last_gap = 0;
   /* The following initialization ensures that the gap flag
      will not be set for the first sm record.
   */
   prev_sm.su = BADFLOAT;
   while (fread(&sm, sizeof(sm), 1, fp_sm) == 1)
   {
      ism++;     /* index of current record, starting from 0 */

      bad = (sm.su >= ADJ_BADFLOAT);
      gap = is_gap(&sm, &prev_sm);

      if (bad)
      {
        if (!last_bad)     iseg++; /* end segment */
/*            end_segment(&iseg); */
      }
      else    /* current point is good; sm has data */
      {
         if ( (gap_boundary = (gap && !last_gap) || (!gap && last_gap) ) == TRUE)
            /*end_segment(&iseg); */ iseg++;
         if (last_bad || gap_boundary)
         {
            /* Start a segment.  Gaps now form independent segments.
               Hence the current segment is ended and a new one begins
               whenever there is a change between gap and no-gap.
            */
            nseg = iseg + 1;    /* number of segments begun */
            if (nseg > segment_array_size)
               enlarge_segment_array();  
            segment[iseg].i0 = ism;            
            segment[iseg].t0 = sm.t - ensemble_time/s_per_day;
            segment[iseg].ex0 = meters_to_dlon(ensemble_time * sm.su, sm.y);
            segment[iseg].ey0 = meters_to_dlat(ensemble_time * sm.sv, sm.y);
            segment[iseg].x0 = sm.x - segment[iseg].ex0;
            segment[iseg].y0 = sm.y - segment[iseg].ey0;
            segment[iseg].n = 1;
            segment[iseg].stdx = 0.0;
            segment[iseg].stdy = 0.0;       
         }
         else        /* continuing a good interval */
         {
            (segment[iseg].n)++;
         }
         segment[iseg].t1 = sm.t;
         segment[iseg].x1 = sm.x;
         segment[iseg].y1 = sm.y;
      }
      prev_sm = sm; 
      last_bad = bad;
      last_gap = gap;
   }
   return nseg;
}                       /* find_segments() */

#if PROTOTYPE_ALLOWED
void print_segment_tables(FILE *fp_log, TRACK_SEGMENT_TYPE *segment, int nseg)
#else
void print_segment_tables(fp_log, segment, nseg)
FILE *fp_log;
TRACK_SEGMENT_TYPE *segment;
int nseg;
#endif
{
   int i;
   double dlon_factor;

   fprintf(fp_log, "\n\nseg     i0    i1      n   nfix");
   for (i=0; i<nseg; i++)
   {
      fprintf(fp_log, "\n %2d  %5ld %5ld   %4ld  %5d", i+1,
               segment[i].i0, segment[i].i0 + segment[i].n, segment[i].n,
               segment[i].nfix);
   }

   fprintf(fp_log, "\n\nseg          t0         t1         x0        x1         y0        y1  stdx  stdy n_fix");
   for (i=0; i<nseg; i++)
   {
      dlon_factor = cos(0.5 * (segment[i].y0 + segment[i].y1) *3.1415/180.0);
      fprintf(fp_log,
               "\n %2d  %10.5f %10.5f  %9.4f %9.4f  %9.4f %9.4f  %4.0f %4.0f %3d",
               i+1,
               segment[i].t0, segment[i].t1, segment[i].x0, segment[i].x1,
               segment[i].y0, segment[i].y1,
               sqrt(segment[i].stdx) * dlon_factor * 1.1e5,
               sqrt(segment[i].stdy) * 1.1e5,segment[i].nfix);
   }
   fprintf(fp_log, "\n");
}


/*

   FUNCTION: adjust_nofix_segments()

      For any segments without fixes, linearly interpolate the positions
      from the adjacent segments.  Let the end of the previous segment,
      the start and end of the middle segment, and the start of the
      following segment be points 0, 1, 2, and 3, respectively.
      If we want the velocity to be equal in the gaps on either side
      of the middle segment, we have (with, e.g., t10 = t1 - t0)

         x1 = ((x3 - x21)t10 + x0 t32) / (t32 + t10)

      End segments lacking fixes must be treated differently:

         x1 = x0 + x21 ( t10/t21 )     last segment
         x2 = x3 - x21 ( t32/t21 )    first segment

      This makes the gap velocity equal to the average
      velocity in the end segment.

      The algorithm has been extended to the case in which segments
      without fixes occur in clusters.

*/

#if PROTOTYPE_ALLOWED
void adjust_nofix_segments(TRACK_SEGMENT_TYPE *segment, int nseg)
#else
void adjust_nofix_segments(segment, nseg)
TRACK_SEGMENT_TYPE *segment;
int nseg;
#endif
{
   int i;
   int nofix, last_nofix;
   int i_nofix, nsegindex;
   SEGINDEX_TYPE *segindex;

   double seg_x, seg_y, seg_t;
   double span_x, span_y, span_t;
   double mean_u, mean_v;         /* degrees per day */
   int j;
   int i_right, i_left;

   segindex = (SEGINDEX_TYPE *)calloc(nseg, sizeof(SEGINDEX_TYPE));
   check_error((segindex == NULL), 
               "Allocation of memory for segindex failed in optpos.c");
   last_nofix = 0;
   i_nofix = 0;
   nsegindex = 0;
   for (i=0; i<nseg; i++)
   {
      nofix = (segment[i].nfix == 0);
      if (nofix && !last_nofix)
      {
         /* start a group of no-fix segments */
         segindex[i_nofix].i0 = i;
         segindex[i_nofix].i1 = i;
         nsegindex++;
      }
      else if (nofix && last_nofix)
      {
         /* still in the group */
         segindex[i_nofix].i1++;
      }
      else if (!nofix && last_nofix)
      {
         /* the group just ended */
         i_nofix++;
      }
      last_nofix = nofix;
   }

#if PDEBUG
   for (i=0; i<nsegindex; i++)
      printf("\n segindex i0= %d  i1= %d", segindex[i].i0, segindex[i].i1);
#endif

   for (i=0; i<nsegindex; i++)
   {
      /* Find the sum of the displacements and time intervals
         of the no-fix segments in the group.
      */
      seg_x = 0.0;
      seg_y = 0.0;
      seg_t = 0.0;
      for (j=segindex[i].i0; j<=segindex[i].i1; j++)
      {
         seg_x += segment[j].x1 - segment[j].x0;
         seg_y += segment[j].y1 - segment[j].y0;
         seg_t += segment[j].t1 - segment[j].t0;
      }

      if (segindex[i].i0 == 0 || segindex[i].i1 == nseg-1)
      {
         /* end point method */
         mean_u = seg_x / seg_t;
         mean_v = seg_y / seg_t;
      }
      else
      {
         /* middle method */
         i_right = segindex[i].i1 + 1;
         i_left = segindex[i].i0 - 1;
         span_x = segment[i_right].x0 - segment[i_left].x1;
         span_y = segment[i_right].y0 - segment[i_left].y1;
         span_t = segment[i_right].t0 - segment[i_left].t1;

         mean_u = (span_x - seg_x) / (span_t - seg_t);
         mean_v = (span_y - seg_y) / (span_t - seg_t);
      }

      /* Now integrate ahead from the previous segment with a
         fix, or, if this group starts with segment 0, integrate
         back from the segment ahead.
      */
      if (segindex[i].i0 == 0)
      {
         for (j=segindex[i].i1; j>=segindex[i].i0; j--)
         {
            /* This assumes that the previous estimate of
               the x1-x0 is usable.
            */
            seg_x = segment[j].x1 - segment[j].x0;
            seg_y = segment[j].y1 - segment[j].y0;
            segment[j].x1 = segment[j+1].x0
                            - (mean_u * (segment[j+1].t0 - segment[j].t1) );
            segment[j].y1 = segment[j+1].y0
                            - (mean_v * (segment[j+1].t0 - segment[j].t1) );
            segment[j].x0 = segment[j].x1 - seg_x;
            segment[j].y0 = segment[j].y1 - seg_y;
         }
      }
      else /* go forward */
      {
         for (j=segindex[i].i0; j<=segindex[i].i1; j++)
         {
            seg_x = segment[j].x1 - segment[j].x0;
            seg_y = segment[j].y1 - segment[j].y0;
            segment[j].x0 = segment[j-1].x1
                            + (mean_u * (segment[j].t0 - segment[j-1].t1) );
            segment[j].y0 = segment[j-1].y1
                            + (mean_v * (segment[j].t0 - segment[j-1].t1) );
            segment[j].x1 = segment[j].x0 + seg_x;
            segment[j].y1 = segment[j].y0 + seg_y;
         }
      }
   }
   free(segindex);

}                       /* adjust_nofix_segments() */

#if PROTOTYPE_ALLOWED
void integrate_segment(FILE *fp_sm, TRACK_SEGMENT_TYPE *segment)
#else
void integrate_segment(fp_sm, segment)
FILE *fp_sm;
TRACK_SEGMENT_TYPE *segment;
#endif
{
   SMOOTHR_OUT_TYPE sm, prev_sm;
   int j;
   double dt;

   /* Reset fp_sm to the beginning of the segment,
      and read one record.
   */
   if (fseek(fp_sm, segment->i0 * sizeof(sm), 0) != 0)
      printf("\nerror in second fseek in optimize_positions\n");
   if (fread(&sm, sizeof(sm), 1, fp_sm) != 1) puts("fread error");

   sm.x = lon_180(segment->x0 + segment->ex0);
   sm.y = segment->y0 + segment->ey0;
   segment->x1 = sm.x;
   segment->y1 = sm.y;

   /* Back up to the same record (first in the segment) and write
      the adjusted position.
   */
   if (fseek(fp_sm, segment->i0 * sizeof(sm), 0) != 0)
      printf("\nerror in third fseek in optimize_positions\n");
   if (fwrite(&sm, sizeof(sm), 1, fp_sm) != 1) puts("fwrite error");

   /* Set previous to current, and go on to the next ensemble in sm. */
   prev_sm = sm;    /* structure assignment */
   for (j=1; j<segment->n; j++)
   {
      double mean_y;
      /* Get the current record. */
      if (fseek(fp_sm, (segment->i0 + j) * sizeof(sm), 0))
         puts("\nerror in fourth fseek in optimize_positions\n");
      if (fread(&sm, sizeof(sm), 1, fp_sm) != 1) puts("fread error");

      /* Integrate x and y from previous time to present time,
         using prev_sm as the accumulator.
      */
      dt = (sm.t - prev_sm.t) * s_per_day;
      mean_y = 0.5 * (prev_sm.y + sm.y);
      prev_sm.x += meters_to_dlon( (sm.su * dt), mean_y);
      prev_sm.x = lon_180(prev_sm.x);
      prev_sm.y += meters_to_dlat( (sm.sv * dt), mean_y);
      prev_sm.t =  sm.t;

      /* Copy the new position into sm, back up the file to
         the current record, and write it out.
      */
      sm.x = prev_sm.x;
      sm.y = prev_sm.y;
      segment->x1 = sm.x;
      segment->y1 = sm.y;

      if (fseek(fp_sm, (segment->i0 + j) * sizeof(sm), 0))
         printf("\nerror in fifth fseek in optimize_positions\n");
      if (fwrite(&sm, sizeof(sm), 1, fp_sm) != 1) puts("fwrite error");
   }  /* End of loop through ensembles after the first one. */

}                       /* integrate_segment() */


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

#if PROTOTYPE_ALLOWED
void optimize_positions(FILE *fp_sm, FILE *fp_fix, FILE *fp_log, 
                        double fix_to_dr_limit, int iterations)
#else
void optimize_positions(fp_sm, fp_fix, fp_log, fix_to_dr_limit, iterations)
FILE *fp_sm, *fp_fix, *fp_log;
double fix_to_dr_limit;
int iterations;
#endif
      /* can put in a step, so that not every GPS fix is taken */
{

   UNISTAT_TYPE fix_stat;
   REFABS_TYPE fix;
   SMOOTHR_OUT_TYPE sm;
   int nseg;
   int i, it;
   int finished;
   POSITION_TYPE dif, dr;  /* difference (fix - dr) and dead-reckoned */
   double dt;
   int fix_header_printed;    /* flag, so header is printed only once */

#if PDEBUG
   fp_log = stdout;
#endif
   allocate_unistat(&fix_stat, 2, "fix", U_ALL);

   /* Find the ranges of times over which to integrate
      velocity.
   */
   printf("\nOptimizing positions: finding continuous data ranges.");
   rewind(fp_sm);
   nseg = find_segments(fp_sm);

   printf("\nThere are %d segments for position integration.", nseg);
   fprintf(fp_log, "\nThere are %d segments for position integration.", nseg);

   /* In each segment, find the average x and y difference between
      the fixes and the dead-reckoned positions at the fix times.
   */
   for (it = 0; it < iterations; it++)
   {
      printf("\nOptimization iteration %d of %d", it+1, iterations);
      fprintf(fp_log, "\nOptimization iteration %d of %d", it+1, iterations);
      rewind(fp_fix);
      fix.t = -BADFLOAT;   /* initialize so a first fix will be read */
      for (i=0; i<nseg; i++)
      {
         zero_unistat(&fix_stat);
         segment[i].dx = 0.0;
         segment[i].dy = 0.0;

         fix_header_printed = 0;


         /* Position the sm file at the start of the segment, and read
            that sm record so that sm.t will be available for testing
            later.
         */
         if (fseek(fp_sm, segment[i].i0 * sizeof(sm), 0) != 0)
            printf("\nerror in first fseek in optimize_positions\n");
         finished = ( fread(&sm, sizeof(sm), 1, fp_sm) != 1 );

         /* Position the fix file to the first fix after the
            start of the segment. Recall that
            fix.t was initialized at the beginning of the
            iteration to a large negative value so that the
            first fix will always be read.
         */
         while ( fix.t < segment[i].t0 && !finished )
            finished = read_refabs(fp_fix, &fix, ALL_REFABS);


         /* For each fix in the segment time range, interpolate
            a dead-reckoned position from the sm file, and
            calculate the mean difference.  The new fix will
            be read at the END of this loop.
         */
         while ( fix.t <= segment[i].t1 && !finished )
         {
            pdebug1(0, "\nfix to be used: %lf", fix.t);
            /* Find the first smoothed record at or after the current fix. */
            while (sm.t < fix.t && !finished)
               finished = ( fread(&sm, sizeof(sm), 1, fp_sm) != 1 );

            /* The dead-reckoned position should be during the ADCP
               ensemble that ends with the time sm.t.  If velocity is
               available in sm, calculate the distance from the
               cruise track to the fix.
            */
            if (sm.su <= ADJ_BADFLOAT)
            {
               dt = (fix.t - sm.t) * s_per_day;
               dr.x = lon_180(sm.x + meters_to_dlon(dt * sm.su, fix.y));
               dr.y = sm.y + meters_to_dlat(dt * sm.sv, fix.y);
               dif.x = lon_180(fix.x - dr.x);
               dif.y = fix.y - dr.y;
               update_unistat_d(&fix_stat, &(dif.x), 2);

               /* Optionally print out the fixes for which the
                  distance from the dr exceeds a threshold.  This
                  need be done only on the last iteration.
               */
               if (it == iterations -1)
               {
                  if (!fix_header_printed)
                  {
                     fprintf(fp_log, "\n\n   Segment %d of %d", i+1, nseg);
                     fprintf(fp_log, "\n   fix time          dx          dy   (1.e-5 degrees)");
                     fix_header_printed = 1;
                  }

                  if ( sqrt(dif.x * dif.x + dif.y * dif.y) >= fix_to_dr_limit )
                     fprintf(fp_log, "\n %10.5f  %10.0f  %10.0f",
                              fix.t, dif.x * 1.e5 , dif.y * 1.e5);
               }
            }
            if (!finished)
               finished = read_refabs(fp_fix, &fix, ALL_REFABS);
         }  /* End of loop through fixes (and through sm file in parallel) */

         calculate_unistat(&fix_stat);
         segment[i].nfix = fix_stat.npts[0];
         /* 95/05/04: added check for nfix > 0; otherwise BADFLOATs
                      from calculate_unistat get added to segment.x0,y0 */
         if (segment[i].nfix > 0)
         {
            /* Add standard deviations to segment table. */
            segment[i].stdx = fix_stat.sumsq[0];
            segment[i].stdy = fix_stat.sumsq[1];
            /* Correct the starting position for the segment. */
            segment[i].x0 += fix_stat.sum[0];
            segment[i].y0 += fix_stat.sum[1];
         }
         pdebug1(0, "\nnfix= %d", segment[i].nfix);

         fprintf(fp_log, "\n\nSegment %d of %d", i+1, nseg);
         fprintf(fp_log, "\n   Time range:  %11.5f to %11.5f",
                        segment[i].t0, segment[i].t1);
         fprintf(fp_log, "\n   Index range: %ld through %ld",
                        segment[i].i0, segment[i].i0 + segment[i].n - 1);
         write_unistat(fp_log, &fix_stat, 0.01, (float *) NULL, "x, y", 0);
      }
      /* End of calculation of differences between fixes and dr positions. */


      /* Now, recalculate the positions in fp_sm. */
      for (i=0; i<nseg; i++)
      {
         pdebug1(1, "\n fix segment %d", i);
#if PDEBUG
         print_segment_tables(stdout, &segment[i], 1);
#endif
         if (segment[i].nfix > 0)
            integrate_segment(fp_sm, &segment[i]);
#if PDEBUG
         print_segment_tables(stdout, &segment[i], 1);
#endif
      }
      adjust_nofix_segments(segment, nseg);
      for (i=0; i<nseg; i++)
      {
         pdebug1(1, "\n nofix segment %d", i);
#if PDEBUG
         print_segment_tables(stdout, &segment[i], 1);
#endif
         if (segment[i].nfix == 0)
            integrate_segment(fp_sm, &segment[i]);
#if PDEBUG
         print_segment_tables(stdout, &segment[i], 1);
#endif
      }


   }  /* End of loop through optimization iterations. */

   print_segment_tables(fp_log, segment, nseg);
   free(segment);
   free_unistat(&fix_stat);
}                       /* optimize_positions() */