/*
This program reads a SeaWiFS level-1a file and outputs an unmapped,
true-color, PPM image of the scene.  The resolution of the output
image is the same as that of the input file.

Norman Kuring		17-Nov-1997

I added an extra argument (ninc) to the calibrate_l1a() function
call to bring this program in line with Joel Gales' latest modification
to that function.
Norman Kuring		30-Oct-2000
*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "mfhdf.h"
#include "call1a_proto.h"

#define USAGE "Usage: %s SeaWiFS_level_1a_filename\n"

#define NUMBANDS	8

#define L1_412          0
#define L1_443          1
#define L1_490          2
#define L1_510          3
#define L1_555          4
#define L1_670          5
#define L1_765          6
#define L1_865          7

#define PI		3.14159265358979323846

#define READ_GLBL_ATTR(nam,ptr) {                                           \
  if(SDreadattr(sd_id,SDfindattr(sd_id,(nam)),(VOIDP)(ptr))){               \
    fprintf(stderr,                                                         \
    "-E- %s line %d: Could not get global attribute, %s, from file, %s.\n", \
    __FILE__,__LINE__,(nam),argv[1]);                                       \
    exit(EXIT_FAILURE);                                                     \
  }                                                                         \
}

#define MALLOC(ptr,typ,num) {                                           \
  (ptr) = (typ *)malloc((num) * sizeof(typ));                           \
  if((ptr) == NULL){                                                    \
    fprintf(stderr,"-E- %s line %d: Memory allocation failure.\n",      \
    __FILE__,__LINE__);                                                 \
    exit(EXIT_FAILURE);                                                 \
  }                                                                     \
}

#define READ_SDS(nam,ptr,s0,s1,s2,e0,e1,e2) {                           \
  int32 start[3];                                                       \
  int32 edge[3];                                                        \
  edge[0]=(e0); edge[1]=(e1); edge[2]=(e2);                             \
  start[0]=(s0); start[1]=(s1); start[2]=(s2);                          \
  if(SDreaddata(SDselect(sd_id, SDnametoindex(sd_id, (nam))),           \
  start, NULL, edge, (VOIDP)(ptr)) == FAIL){                            \
    fprintf(stderr,"-E- %s line %d: Could not read SDS, %s.\n",         \
    __FILE__,__LINE__,(nam));                                           \
    exit(EXIT_FAILURE);                                                 \
  }                                                                     \
}

void ray(int32, int32 a[NUMBANDS], float32,
         float32 *, float32 *, float32 *, float32 *, float32 *b[NUMBANDS]);
void geonav(float32 *orb_vec, float32 *sen_mat, float32 *scan_ell,
            float32 *sun_ref, int32 startpix, int32 subsamp, int32 npix,
            float32 *lat, float32 *lon, float32 *solz, float32 *sola,
            float32 *senz, float32 *sena);
unsigned char scale_datum(float32);
int i16comp(const void *, const void *);

void
main(int argc, char *argv[]){

  FILE		*fh;
  static int32	bandflag[NUMBANDS] = {1,0,0,0,1,1,0,0}; /* bands 1, 5, an 6 */
  char		*cal_path;
  int32		sd_id;
  int16		syear,sday,eday;
  int32		msec,smsec,npix,nscan,lacstart,lacsub;
  char		datatype[32];
  int16		*l1a_data,*l1a_reordered,*dark_rest,*d_rest[NUMBANDS];
  float32	*lat,*lon,*solz,*sola,*senz,*sena,*l1b,*l1b_sub[NUMBANDS],dsol;
  float32	dark_median[NUMBANDS];
  float32	orb_vec[3],sun_ref[3],sen_mat[9],scan_ell[6];
  int16		gain[NUMBANDS],tdi[NUMBANDS],scan_temp[NUMBANDS],side;
  int32		s,p,b,i,n;
  cal_mod_struc	cal_mod;
  unsigned char	rgb[3];
  int32		progress;

  if(argc != 2){
    fprintf(stderr,USAGE,argv[0]);
    exit(EXIT_FAILURE);
  }

  /* Make sure the input file exists. */
  if((fh = fopen(argv[1],"rb")) == NULL){
    fprintf(stderr,"-E- %s line %d: Could not open file, %s .  ",
    __FILE__,__LINE__,argv[1]);
    perror(NULL);
    exit(EXIT_FAILURE);
  }
  else{
    fclose(fh);
  }

  /* Get the calibration table filename */
  cal_path = getenv("CAL_HDF_PATH");
  if(cal_path == NULL){
    fprintf(stderr,
    "-E- %s line %d: Environment variable, CAL_HDF_PATH, is not set.\n",
    __FILE__,__LINE__);
    exit(EXIT_FAILURE);
  }

  /* Open the HDF input file */
  sd_id = SDstart(argv[1], DFACC_RDONLY);
  if(sd_id == FAIL){
    fprintf(stderr,"-E- %s line %d: SDstart(%s, %d) failed.\n",
    __FILE__,__LINE__,argv[1],DFACC_RDONLY);
    exit(EXIT_FAILURE);
  }

  /* Read level-1A global attributes. */
  READ_GLBL_ATTR("Start Year"            ,&syear);
  READ_GLBL_ATTR("Start Day"             ,&sday);
  READ_GLBL_ATTR("Start Millisec"        ,&smsec);
  READ_GLBL_ATTR("End Day"               ,&eday);
  READ_GLBL_ATTR("Data Type"             ,datatype);
  READ_GLBL_ATTR("Pixels per Scan Line"  ,&npix);
  READ_GLBL_ATTR("Number of Scan Lines"  ,&nscan);
  READ_GLBL_ATTR("LAC Pixel Start Number",&lacstart);
  READ_GLBL_ATTR("LAC Pixel Subsampling" ,&lacsub);

  /* Allocate some memory. */
  MALLOC(l1a_data     , int16  , npix * 8);
  MALLOC(l1a_reordered, int16  , npix * 8);
  MALLOC(lat          , float32, npix);
  MALLOC(lon          , float32, npix);
  MALLOC(solz         , float32, npix);
  MALLOC(sola         , float32, npix);
  MALLOC(senz         , float32, npix);
  MALLOC(sena         , float32, npix);
  MALLOC(l1b          , float32, npix * 8);
  for(b = 0; b < NUMBANDS; b++){
    MALLOC(l1b_sub[b] ,float32,npix);
  }

  /* dsol is needed in the Rayleigh computations. See ray.c */
  dsol=(0.9856*(sday-4))*PI/180.0;
  dsol=1.0/(pow(1.0 - 0.01673*cos(dsol), 2));

  /*
  Get the median dark restore value for each of the 8 bands in the input file.
  Pass these medians to calibrate_l1a() instead of mean values so as to
  avoid using dark restore values corrupted by bit errors.
  Filter out scans having high or low dark restore values before finding
  the median.
  */
  MALLOC(dark_rest, int16, nscan * NUMBANDS);
  READ_SDS("dark_rest", dark_rest, 0,0,0, nscan,NUMBANDS,1);
  for(b = 0; b < NUMBANDS; b++){
    MALLOC(d_rest[b], int16, nscan);
  }
  for(s = 0, n = 0; s < nscan; s++){
    for(b = 0; b < NUMBANDS; b++){
      i = NUMBANDS * s + b;
      if(dark_rest[i] <  5 || dark_rest[i] > 35) break;
    }
    if(b != NUMBANDS) continue;
    for(b = 0; b < NUMBANDS; b++){
      i = NUMBANDS * s + b;
      d_rest[b][n] = dark_rest[i];
    }
    n++;
  }
  for(b = 0; b < NUMBANDS; b++){
    qsort(d_rest[b], n, sizeof(int16), &i16comp);
    dark_median[b] = d_rest[b][n/2];
    free(d_rest[b]);
  }

  cal_mod.flag = 0;

  /* Output a PPM header for this image. */
  printf("P6\n%d %d\n255\n",npix,nscan);

  /* Use the following variable to show this program's progress. */
  progress = (int)ceil(((double)nscan/78));

  /* Loop through the scan lines. */
  for(s = 0; s < nscan; s++){

    if(s % progress == 0) fprintf(stderr,".");

    /* Read this scan's data. */
    READ_SDS("msec"     ,          &msec, s,0,0, 1,1,1);
    READ_SDS("orb_vec"  ,        orb_vec, s,0,0, 1,3,1);
    READ_SDS("sun_ref"  ,        sun_ref, s,0,0, 1,3,1);
    READ_SDS("sen_mat"  ,        sen_mat, s,0,0, 1,3,3);
    READ_SDS("scan_ell" ,       scan_ell, s,0,0, 1,6,1);
    READ_SDS("l1a_data" ,       l1a_data, s,0,0, 1,npix,NUMBANDS);
    READ_SDS("gain"     ,           gain, s,0,0, 1,NUMBANDS,1);
    READ_SDS("tdi"      ,            tdi, s,0,0, 1,NUMBANDS,1);
    READ_SDS("scan_temp",      scan_temp, s,0,0, 1,NUMBANDS,1);
    READ_SDS("side"     ,          &side, s,0,0, 1,1,1);

    /*
    Change the ordering of the level-1a data because calibrate_l1a()
    expects the data to be band-interleaved-by-line while the data
    is stored in the HDF file as band-interleaved-by-pixel.
    Don't ask me why; I only work here.
    */
    for(p = 0; p < npix; p++){
      for(b = 0; b < NUMBANDS; b++){
        l1a_reordered[b * npix + p] = l1a_data[p * NUMBANDS + b];
      }
    }

    /* Calibrate the data. */
    if(
    calibrate_l1a(cal_path,syear,sday,smsec,eday,msec,datatype,1,1,npix,
    &dark_rest[NUMBANDS * s],dark_median,
    gain,tdi,scan_temp,side,l1a_reordered,l1b,&cal_mod)
    < 0){
      fprintf(stderr,"-E- %s line %d: calibrate_l1a() failed for file, %s.\n",
      __FILE__,__LINE__,argv[1]);
      exit(EXIT_FAILURE);
    }

    /* Navigate the data. */
    geonav(orb_vec, sen_mat, scan_ell, sun_ref,
           lacstart, lacsub, npix, lat, lon, solz, sola, senz, sena);

    /* Fill in the input values for the ray() function. */
    for(p = 0; p < npix; p++){
      for(b = 0; b < NUMBANDS; b++){
        if(bandflag[b] == 0) continue;
        l1b_sub[b][p] = l1b[b * npix + p];
      }
    }

    /* Do a Rayleigh correction. */
    ray(npix,bandflag,dsol,solz,sola,senz,sena,l1b_sub);

    /* Scale the data and send it to standard output. */
    for(p = 0; p < npix; p++){
      rgb[0] = scale_datum(l1b_sub[L1_670][p]);
      rgb[1] = scale_datum(l1b_sub[L1_555][p]);
      rgb[2] = scale_datum(l1b_sub[L1_412][p]);
      fwrite(rgb,1,3,stdout);
    }
  }
  fprintf(stderr,"\n");

  free(l1a_data);
  free(l1a_reordered);
  free(lat);
  free(lon);
  free(solz);
  free(sola);
  free(senz);
  free(sena);
  free(l1b);
  free(dark_rest);
  for(b = 0; b < NUMBANDS; b++){
    free(l1b_sub[b]);
  }
}




#define PIXARCWIDTH	0.0015911	/* radians */
#define NADIRPIXEL	643		/* one-based */
#define FLATTENING	(1.0 / 298.257)

/* This number is somewhat arbitrary, I think.  I just picked the value
   to be consistent with the one Fred uses in his Fortran routine. */
#define SMALL_ANGLE	(0.05 * PI / 180.0)

#define A       scan_ell[0]
#define B       scan_ell[1]
#define C       scan_ell[2]
#define D       scan_ell[3]
#define E       scan_ell[4]
#define F       scan_ell[5]

/* Interpreted in row-major order, the HDF browse file actually stores
*  the transpose of the sensor orientation matrix described in the Patt
*  and Gregg reference.  The form of the macros below is Mrc, where r
*  is the row and c is the column of the original matrix, M, described
*  by Patt and Gregg. */
#define M11     sen_mat[0]
#define M12     sen_mat[3]
#define M13     sen_mat[6]
#define M21     sen_mat[1]
#define M22     sen_mat[4]
#define M23     sen_mat[7]
#define M31     sen_mat[2]
#define M32     sen_mat[5]
#define M33     sen_mat[8]

#define Px      orb_vec[0]
#define Py      orb_vec[1]
#define Pz      orb_vec[2]

void geonav(
float32	*orb_vec,
float32	*sen_mat,
float32	*scan_ell,
float32	*sun_ref,
int32	startpix,
int32	subsamp,
int32	npix,
float32	*lat,
float32	*lon,
float32	*solz,
float32	*sola,
float32	*senz,
float32	*sena
){

  static int32	p_startpix = 0, p_subsamp = 0, p_npix = 0;
  static double	*sina=NULL, *cosa=NULL, *sin2a=NULL, *cos2a=NULL, *sincosa=NULL;
  static double	elev, sinl, cosl, oopcf; /* out-of-plane correction factor */
  static double	omf2 = (1.0 - FLATTENING)*(1.0 - FLATTENING);
  int32		i, j;
  double	a, b, c, b2m4ac, q, Qx, Qy, Qz, Gx, Gy, Gz;
  double	up[3], north[3], east[3], upxy, magnitude, rmtq[3];
  double	senv, senn, sene, solv, soln, sole;

  if(
  startpix != p_startpix ||
  subsamp  != p_subsamp  ||
  npix     != p_npix
  ){
    /*
    Pixel configuration has changed.  Recompute sines, etc.
    */
    double	alpha;

    /* Compute elevation (out-of-plane) angle. */
    elev = 1.2 * PIXARCWIDTH;
    sinl = sin(elev);
    cosl = cos(elev);

    if(npix != p_npix){
      if(sina    != NULL) free(sina);
      if(cosa    != NULL) free(cosa);
      if(sin2a   != NULL) free(sin2a);
      if(cos2a   != NULL) free(cos2a);
      if(sincosa != NULL) free(sincosa);
      MALLOC(sina   , double, npix);
      MALLOC(cosa   , double, npix);
      MALLOC(sin2a  , double, npix);
      MALLOC(cos2a  , double, npix);
      MALLOC(sincosa, double, npix);
    }

    for(i = 0; i < npix; i++){
      alpha =
      (double)(startpix - NADIRPIXEL + i * subsamp) * PIXARCWIDTH;
      sina[i]    = sin(alpha) * cosl;
      cosa[i]    = cos(alpha) * cosl;
      sin2a[i]   = sina[i] * sina[i];
      cos2a[i]   = cosa[i] * cosa[i];
      sincosa[i] = sina[i] * cosa[i];
    }

    p_startpix = startpix;
    p_subsamp  = subsamp;
    p_npix     = npix;
  }

  /* Compute correction factor for out-of-plane angle. */
  oopcf = 2.0 * (M21 * Px + M22 * Py + M23 * Pz / omf2);

  for(i = 0; i < npix; i++){

    /* Coefficients of a quadratic equation in q */
    a = A * cos2a[i] + B * sincosa[i] + C * sin2a[i];
    b = D * cosa[i]  + E * sina[i];
    c = F;

    /* The discriminant from the quadratic formula */
    b2m4ac = b * b - 4 * a * c;

    /* Flag pixels that do not fall on the earth */
    if(b2m4ac < 0){
      solz[i] = sola[i] = senz[i] = sena[i] = 9999.9;
      lat[i] = lon[i] = 9999.9;
      continue;
    }

    /* Use the quadratic formula and choose the smaller
    *  of the two solutions to the quadratic equation */
    q = (-b - sqrt(b2m4ac)) / (2 * a);

    /* Apply out-of-plane correction. */
    q *= 1.0 + sinl * oopcf / sqrt(b2m4ac);

    /* Compute the viewing vector */
    Qx = q * cosa[i];
    Qy = q * sinl;
    Qz = q * sina[i];

    /* Intermediate variables. */
    rmtq[0] = M11 * Qx + M21 * Qy + M31 * Qz;
    rmtq[1] = M12 * Qx + M22 * Qy + M32 * Qz;
    rmtq[2] = M13 * Qx + M23 * Qy + M33 * Qz;

    /* Compute the pixel's surface location vector */
    Gx = rmtq[0] + Px;
    Gy = rmtq[1] + Py;
    Gz = rmtq[2] + Pz;

    /* Compute the local vertical, East and North unit vectors. */
    magnitude = sqrt(omf2*omf2*(Gx*Gx + Gy*Gy) + Gz*Gz);
    up[0] = omf2 * Gx / magnitude;
    up[1] = omf2 * Gy / magnitude;
    up[2] =        Gz / magnitude;
    upxy = sqrt(up[0]*up[0] + up[1]*up[1]);
    east[0] = -up[1]/upxy;
    east[1] =  up[0]/upxy;
    east[2] =           0;
    north[0] = up[1]*east[2] - up[2]*east[1];
    north[1] = up[2]*east[0] - up[0]*east[2];
    north[2] = up[0]*east[1] - up[1]*east[0];

    /* Compute components of spacecraft and sun vector in the
    *  vertical, North, and East vectors' frame. */
    senv = senn = sene = 0.0;
    solv = soln = sole = 0.0;
    for(j = 0; j < 3; j++){
      senv -=    rmtq[j] *    up[j];
      senn -=    rmtq[j] * north[j];
      sene -=    rmtq[j] *  east[j];
      solv += sun_ref[j] *    up[j];
      soln += sun_ref[j] * north[j];
      sole += sun_ref[j] *  east[j];
    }

    /* Compute the sensor and solar zenith angles. */
    senz[i] = atan2(sqrt(senn*senn + sene*sene), senv);
    if(senz[i] > SMALL_ANGLE){
      sena[i] = atan2(sene, senn);
      if(sena[i] < 0.0)  sena[i] += 2 * PI;
    }
    else{
      sena[i] = 0.0;
    }
    solz[i] = atan2(sqrt(soln*soln + sole*sole), solv);
    if(solz[i] > SMALL_ANGLE){
      sola[i] = atan2(sole, soln);
      if(sola[i] < 0.0)  sola[i] += 2 * PI;
    }
    else{
      sola[i] = 0.0;
    }

    /* Compute the geodetic latitude and longitude of the pixel */
    lat[i] = atan2(Gz, omf2 * sqrt(Gx*Gx + Gy*Gy));
    lon[i] = atan2(Gy, Gx);
  }
}


unsigned char scale_datum(float val){
  double rint(double);
  double scaledval;
  if(val > 0.01)
    scaledval = rint((log10(val) + 2.0)/(2.0/255));
#ifdef NOTCOMMENTTEDOUT
  /* Use this scaling to bring out water features. */
  if(val > 0.01)
    scaledval = rint((log10(val) + 2.0)/(1.0/255));
  /* This scaling works well for bright areas (i.e. desert, clouds, ice). */
  if(val > -0.99)
    scaledval = rint(log10(val + 0.99)/0.001171973);
  if(val > -0.1)
    scaledval = rint((log10(val+0.1) + 1)/0.004174345);
#endif
  else
    return(0);
  if(scaledval <   0) return(0);
  if(scaledval > 255) return(255);
  return((unsigned char)scaledval);
}


int i16comp(const void *a, const void *b){
  return(*(int16 *)a - *(int16 *)b);
}
