#include <stdio.h>
#include <math.h>   /* fabs() */
#include <string.h>
#include "ioserv.h" /* OPTION_TYPE, etc. */

#define to_radians(deg)   (deg) * 0.01745329251994              /* PI/180.0 */
#define equal(x, y)       (fabs( (x) - (y) ) <= 0.0001 ? 1 : 0) /* some epsilon */
#define good_float(x)     (x) < 1E37                   /* some large number */

#if PROTOTYPE_ALLOWED
void do_it(FILE *fpcnt)
#else
void do_it(fpcnt)
FILE *fpcnt;
#endif
{

   FILE *fpnav, *fphdg = NULL, *fpout, *fpang = NULL;
   static char navfile[180] = "", hdgfile[180] = "", outfile[180] = "rotnav.out",
	       angfile[180] = "";
   static double amp = 1.0, a0 = 0.0, cosH = 0.0, sinH = 0.0, ang = 0.0;
   double hdg, hdg_rad, sinalfa, cosalfa, alfa_rad, alfa_deg, u, v, 
	  hdgtime, navtime, angtime;
   double depth;
   int nfields;	  
   char navbuf[80], hdgbuf[80], angbuf[80];

   static OPTION_TYPE options[] =
   {
      "end"                , 0          , NULL        , (char *) NULL,
      "INPUT_NAV_FILE:"    , TYPE_STRING, op_get_param, navfile,
      "INPUT_HEADING_FILE:", TYPE_STRING, op_get_param, hdgfile,
      "INPUT_ANGLE_FILE:"  , TYPE_STRING, op_get_param, angfile,
      "OUTPUT_NAV_FILE:"   , TYPE_STRING, op_get_param, outfile,
      "amp="               , TYPE_DOUBLE, op_get_param, (char *) &amp,
      "a0="                , TYPE_DOUBLE, op_get_param, (char *) &a0,
      "cosH="              , TYPE_DOUBLE, op_get_param, (char *) &cosH,
      "sinH="              , TYPE_DOUBLE, op_get_param, (char *) &sinH,
      NULL                 , 0          , NULL        , (char *) NULL
   };

   if ((fpout = fopen(outfile, "w")) == NULL)
   {
      fprintf(stderr, "\n ERROR: Unable to open output file %s.\n\n", outfile);
      return;
   }
   set_msg_file(fpout);

   if (execute_options(fpcnt, options, ECHO) <= 0) return;
   check_error(!strcmp(navfile, ""), "no navigation input file!");
   check_error( (amp == 1.0 && a0 == 0.0 && cosH == 0.0 && sinH == 0.0 && !strcmp(angfile, "") ),
      "nothing to do!" );
   check_error( (( cosH != 0.0 || sinH != 0.0) & !strcmp(hdgfile, "")),
      "cannot do heading rotation without input heading file");

   fpnav = check_fopen(navfile, "r");
   if (strcmp(hdgfile, "")) fphdg = check_fopen(hdgfile, "r");
   if (strcmp(angfile, "")) fpang = check_fopen(angfile, "r");

   fputs("%        day   reference_bins\n", fpout);

   while (getline_nc(fpnav, navbuf, 80) != EOF)
   {
      nfields = sscanf(navbuf, " %lf %lf %lf %lf", &navtime, &u, &v, &depth);
      if ( nfields < 3)
      {
         fprintf(stderr, "\n ERROR: Scanning input navigation line:\n %s\n",
            navbuf);
         return;
      }
      alfa_deg = a0;  /* for constant rotation */
      if (fphdg)
      {
         while (getline_nc(fphdg, hdgbuf, 80) != EOF &&
            sscanf(hdgbuf, " %lf %lf", &hdgtime, &hdg) == 2 &&
            !equal(hdgtime, navtime)); /* match times */
         if (feof(fphdg))
         {
            fprintf(stderr, "\n ERROR: No heading to match navigation time %f",
               navtime);
            break;
         }
         hdg_rad  = to_radians(hdg);
         alfa_deg += cosH * cos(hdg_rad) + sinH * sin(hdg_rad);
      }
      if (fpang)
      {
         while (getline_nc(fpang, angbuf, 80) != EOF &&
            sscanf(angbuf, " %lf %lf", &angtime, &ang) == 2 &&
            !equal(angtime, navtime)); /* match times */
         if (feof(fpang))
         {
            fprintf(stderr, "\n ERROR: No angle to match navigation time %f",
               navtime);
            break;
         }
	 alfa_deg += ang;
      }

      alfa_rad = to_radians(alfa_deg);
      sinalfa = sin(alfa_rad);
      cosalfa = cos(alfa_rad);

      if (good_float(u))
      {
         fprintf(fpout, "%12.6f  %7.3f %7.3f", navtime,
            amp * (u * cosalfa - v * sinalfa),
            amp * (u * sinalfa + v * cosalfa));
         if (nfields == 4)
            fprintf(fpout, "  %5.0f", depth);
         fprintf(fpout, "\n");      
      }
      else
         fputs(navbuf, fpout);
   }

   fclose(fpout);
   if (strcmp(outfile, "rotnav.out"))
      rename("rotnav.out", outfile);
   fputs("\n\n", stderr);
}