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

   SHIPREF.C module in ADCPSECT

   Routines for getting ship and/or reference velocity;
   the navigation and reference options in ADCPSECT.



*/

#include "geninc.h"
#include "ioserv.h"
#include "vector.h"
#include "use_db.h"
#include "vstat.h"
#include "data_id.h"
#include "pos_to_m.h"
#include "adcp.h"
#include "adcpsect.h"

#ifndef M_PI
#define M_PI		3.14159265358979323846
#endif
extern int first_profile;    /* flag: true if first profile in ensemble
      Check whether it should be external, static, or other.<<<<<*/

extern   char out_filename[];
static char  nav_filename[NPATHMAX];
static   FILE  *fp_nav;
         PROFILE_REF_TYPE ref = {REF_NONE, {0, 0}};
         PROFILE_REF_TYPE nav = {REF_NONE, {0, 0}};


static OPTION_TYPE ref_options[] =
{
   {"none"           , REF_NONE           , set_code    , (char*)&ref.method},
   {"final_ship_ref" , REF_FINAL_SHIP     , set_code    , (char*)&ref.method},
   {"nav_position"   , REF_NAV_POS        , set_code    , (char*)&ref.method},
   {"nav_velocity"   , REF_NAV_VEL        , set_code    , (char*)&ref.method},
   {"bottom_track"   , REF_BOTTOM_TRACK   , set_code    , (char*)&ref.method},
   {"dir_position"   , REF_DIR_POS        , set_code    , (char*)&ref.method},
   {"reference_bins" , REF_BINS           , read_bin_range  , (char *)&ref},
   {NULL             , NULL               , NULL         , NULL}
};

/* The order of the following is the order of the columns in
   the .NAV file, output of the "navigation" option.
*/
static OPTION_TYPE nav_options[] =
{
   {"end"            , 0                  , NULL          , (char*)NULL},
   {"reference_bins" , REF_BINS           , read_bin_range  , (char *)&nav},
   {"bottom_track"   , REF_BOTTOM_TRACK   , logical_or    , (char*)&nav.method},
   {"nav_position"   , REF_NAV_POS        , logical_or    , (char*)&nav.method},
   {"nav_velocity"   , REF_NAV_VEL        , logical_or    , (char*)&nav.method},
   {"final_ship_ref" , REF_FINAL_SHIP     , logical_or    , (char*)&nav.method},
   {"dir_position"   , REF_DIR_POS        , logical_or    , (char*)&nav.method},
   {NULL             , NULL               , NULL         , NULL}
};

#if PROTOTYPE_ALLOWED
int read_bin_range(FILE *fp_nav, char *refc, int code)
#else
int read_bin_range(fp_nav, refc, code)
FILE *fp_nav;
char *refc;
int code;
#endif
{
   PROFILE_REF_TYPE *ref;

   ref = (PROFILE_REF_TYPE *) refc;

   check_error( (fscanf(fp_nav, " %d to %d",
               &(ref->bin_range[0]), &(ref->bin_range[1])) !=2),
               "reading ref bin range");
   ref->method = code;
   return(0);
}

#if PROTOTYPE_ALLOWED
int init_ref(FILE *fp_cnt)
#else
int init_ref(fp_cnt)
FILE *fp_cnt;
#endif
{
   if (execute_one_option(fp_cnt, ref_options) <= 0)
   {
      printf("\n ERROR in reading reference options.");
      exit(-1);
   }
   return(0);
}

#if PROTOTYPE_ALLOWED
int init_nav(FILE *fp_cnt)
#else
int init_nav(fp_cnt)
FILE *fp_cnt;
#endif
{
   int i;

   if (execute_options(fp_cnt, nav_options, ECHO) <= 0)
   {
      printf("\n ERROR in reading navigation options.");
      exit(-1);
   }
   /* Make a nav output file name, and open the file. */
   new_extension(nav_filename, out_filename, ".nav");
   fp_nav = check_fopen(nav_filename, "w");
   fprintf(fp_nav, "%%        day  ");
   for (i=1; nav_options[i].code != NULL; i++)
   {
      if (nav.method & nav_options[i].code)
      {
         fprintf(fp_nav, "%15s    ", nav_options[i].name);
      }
   }
   fprintf(fp_nav, "\n");

   return(0);
}

#if PROTOTYPE_ALLOWED
void get_ship_velocity(ADCP_PROFILE_TYPE *p, PROFILE_REF_TYPE *ref, 
                       FIX_TYPE *last_fix, double *ship_u_ptr, double *ship_v_ptr)
#else
void get_ship_velocity(p, ref, last_fix, ship_u_ptr, ship_v_ptr)
ADCP_PROFILE_TYPE *p;
PROFILE_REF_TYPE *ref;
FIX_TYPE *last_fix;
double *ship_u_ptr, *ship_v_ptr;
#endif
{
   extern int verbose;

   int nb;           /* number of bytes read */
   int nbins;        /* number of bins for reference range */
   int good_fix;     /* flag */
   int good_vel;     /* flag */
   BOTTOM_TRACK_TYPE bottom_track;   /* only needed locally for now */
   NAVIGATION_TYPE nav;
   double dx, dy; /* present minus last lon, lat, in meters */
   double dt;    /* difference between present and last time */
   int bin0, bin1;

   if (verbose) printf(" ref->method: %d ", ref->method);
   switch (ref->method)
   {
      case REF_NONE:
         break;            /* no ship velocity will be calculated */
      case REF_FINAL_SHIP:
         *ship_u_ptr = p->access.U_ship_absolute;
         *ship_v_ptr = p->access.V_ship_absolute;
         return;
      case REF_BINS:
         bin0 = max_val(ref->bin_range[0], p->access.first_good_bin);
         bin1 = min_val(ref->bin_range[1], p->nu - 1);
         /* p->nu is already the minimum of the number of bins
            of profile data read + 1, and (access.last_good_bin + 1).
            Recall that bin zero is filled with BADFLOAT, and then
            the data are read into arrays starting with index 1 for
            bin 1.  So, the 1-based bin numbers also correspond to
            the indices in the 0-based C arrays.
         */
         if ( ref->bin_range[1] != bin1  ||
              ref->bin_range[0] != bin0 )
         {
            printf("Warning at ");
            write_ymdhms_time(stdout, &(p->time));
            printf(" bin range is %d to %d\n",
                    p->access.first_good_bin,
                    p->nu - 1);
         }
         nbins  = 1 + bin1 - bin0;
         *ship_u_ptr = - array_mean(p->u + bin0, nbins);
         *ship_v_ptr = - array_mean(p->v + bin0, nbins);
         if (*ship_u_ptr <= -ADJ_BADFLOAT)
         {
            *ship_u_ptr = BADFLOAT;
            *ship_v_ptr = BADFLOAT;
         }
         return;
      case REF_BOTTOM_TRACK:
         nb = check_dbget(BOTTOM_TRACK, (char *)&bottom_track,
                     sizeof(bottom_track), "dbget bottom track");

         if(verbose) printf("\n BT: nbytes= %d, u= %f, v= %f, d= %f\n",
                              nb, bottom_track.u,
                              bottom_track.v,
                              bottom_track.depth);

         if (nb == sizeof(bottom_track))
/*            if (bottom_track.u < 19999.0)  jr+- */
            if (bottom_track.u < ADJ_BADFLOAT)
            {
               *ship_u_ptr = - bottom_track.u;
               *ship_v_ptr = - bottom_track.v;
               return;     /* successful */
            }
         printf("\nno bottom track");
         break;   /* bottom track ref not available */
      case REF_NAV_VEL:
         nb = check_dbget(NAVIGATION, (char *)&nav,
                     sizeof(nav), "dbget navigation");

         if(verbose) printf("\n NAV VEL: nbytes= %d, spd= %f, dir= %f\n",
                              nb, nav.speed,  nav.direction);
         if (fabs(nav.speed) < 20)   /* anything over 20 kts must be bad */
         {
            *ship_u_ptr = MPS_PER_KT * nav.speed
                                     * sin(nav.direction * M_PI/180);
            *ship_v_ptr = MPS_PER_KT * nav.speed
                                     * cos(nav.direction * M_PI/180);
            return;
         }
         else  break;
      case REF_NAV_POS:
         nb = check_dbget(NAVIGATION, (char *)&nav,
                     sizeof(nav), "dbget navigation");

         if(verbose) printf("\n NAV POS: nbytes= %d, lat= %f, lon= %f\n",
                              nb, nav.latitude, nav.longitude);
         if (fabs(nav.latitude) > 90)  good_fix = FALSE;
         else                           good_fix = TRUE;

         if (first_profile)  last_fix->good = FALSE;

         if (good_fix && last_fix->good)
         {
            dt = (double) TIMDIF(&last_fix->time, &p->time);
            dx = dlon_to_meters(nav.longitude - last_fix->lon,
                                                nav.latitude);
            dy = dlat_to_meters(nav.latitude - last_fix->lat,
                                                nav.latitude);
            if (dt != 0.0)
            {
               *ship_u_ptr = dx/dt;
               *ship_v_ptr = dy/dt;
               good_vel = TRUE;
            }
         }
         else  good_vel = FALSE;
         if (!good_fix)
         {
            last_fix->good = FALSE;
         }
         else
         {
            last_fix->time = p->time;
            last_fix->lat = nav.latitude;
            last_fix->lon = nav.longitude;
            last_fix->good = TRUE;
         }
         if (good_vel)  return;
         else break;   /* don't have ship vel est. yet */
      case REF_DIR_POS:    /* not implemented yet, but similar to above */
      default:
         printf("\nUnavailable profile reference option.\n");
   }
   *ship_u_ptr = BADFLOAT;
   *ship_v_ptr = BADFLOAT;
   return;             /* exit here if desired ship ref is unavailable */
}                       /* get_ship_velocity */

/* The reason for using time in year-days is that it is
not too hard to convert back and forth from dates and times
to year-days; one gets a better sense of time with them
than with seconds, for example.
*/
#if PROTOTYPE_ALLOWED
void write_navigation(ADCP_PROFILE_TYPE *p, PROFILE_REF_TYPE *nav)
#else
void write_navigation(p, nav)
ADCP_PROFILE_TYPE *p;
PROFILE_REF_TYPE *nav;
#endif
{
   PROFILE_REF_TYPE r;    /* we need this so we can modify "method" */
   static FIX_TYPE last_fix; /* for REF_NAV_POS only */
   FIX_TYPE dummy;           /* for other calls to get_ship_velocity */
   double ship_u, ship_v;
   int i;

   r.bin_range[0] = nav->bin_range[0];
   r.bin_range[1] = nav->bin_range[1];
   fprintf(fp_nav, "%12.6f  ", p->ddtime);

   for (i=1; nav_options[i].code != NULL; i++)
   {
      if (nav->method & nav_options[i].code)
      {
         r.method = nav_options[i].code;
         if (r.method == REF_NAV_POS)
            get_ship_velocity(p, &r, &last_fix, &ship_u, &ship_v);
         else
            get_ship_velocity(p, &r, &dummy, &ship_u, &ship_v);
         if(good_float(ship_u))
            fprintf(fp_nav, "%7.3f %7.3f    ", ship_u, ship_v);
         else
            fprintf(fp_nav, "%7s %7s    ", "1E38", "1E38");
      }
   }
   fprintf(fp_nav, "\n");
}                       /* write_navigation() */

#if PROTOTYPE_ALLOWED
void close_navigation(void)
#else
void close_navigation()
#endif
{
   fclose(fp_nav);
}
