;+
; NAME:
;       iris_calc_geo()
;
; PURPOSE:
;       A sub-routine of iris_spec_cal.
;
;       Based on the user input of rough pixel positions, this
;       function does Gaussian fitting of the spectral and fiducial
;       lines present in IRIS spectra across the Y and X dimensions of
;       the image array to determine a geometrical solution which
;       makes the spectra and spatial lines straight with respect to
;       the pixel rows and columns, and rectilinear with respect to
;       each other.
;
; CALLING SEQUENCE:
;       result = iris_calc_geo(img, rxy, lxy, fxy, wave='', mask=mask)
;
; INPUTS:
;       img  - The spectrum from which the lines will be fitted.
;       rxy  - An array of the rough xy coordinates for a line center along
;              the y direction of the CCD, used for determining the
;              shape of the window used for fitting.
;       lxy  - An array of approximate line centers for the lines to
;              be used in the geometry calibration.
;       fxy  - An array of approximate fiducial positions (there are
;              typically two fiducials)
;       wave - A string containing the IRIS wavelength ID for the
;              spectrum, either 'nuv', 'fuvs', or 'fuvl'.
;
; OPTIONAL INPUTS:
;       mask - A byte array for the pixels that will be considered in
;              the fit
;
; KEYWORD PARAMETERS:
;       None 
;
; OUTPUTS:
;       Returns a structure containing tags:
;           .kx    - The x array of the 2D polynomial solution
;                    returned by POLYWARP
;           .ky    - The y array of the 2D polynomial solution
;                    returned by POLYWARP
;           .kxi   - The inverse kx solution
;           .kyi   - The inverse ky solution
;           .dmapx - The x component of the distortion map solution
;           .dmapy - The y component of the distortion map solution
;           .xpos  - An array of the fitted x positions provided to POLYWARP
;           .ypos  - An array of the fitted y
;           .xpos0 - An array of the desired x
;           .ypos0 - An array of the desired y
;           .lines
;                 .gaussfit
;                 .polyfit
;           .fiducials
;                     .gaussfit
;                     .polyfit
;
; OPTIONAL OUTPUTS:
;       None
;
; COMMON BLOCKS:
;       None
;
; PROCEDURES USED:
;       POLYWARP, ROBUST_POLYFIT
;
; COMMENTS:
;
; EXAMPLES:
;          IDL> result = iris_calc_geo(img, rxy, lxy, fxy, wave='nuv')
;
; MODIFICATION HISTORY:
;       Started 2013-June-3 by Sarah A. Jaeggli, Montana State University
;
;               2013-June-11 Added error handling for bad fits and
;               reversed the index order for *xy
;
;               2013-July-26 added mask keyword so that overscan and
;               low signal regions can be ignored
;
;               2013-Aug-6 included line and fiducial fit results in
;               return structure, since these are useful for debugging
;               and other applications (flatfields)
;
;               2014-Jun-13 adjusted fit quality thresholding
;
;               2014-July-28 SAJ standardized FUV1, FUV2 labeling to
;               match image header standards
;-

FUNCTION iris_calc_geo, img, rxy, lxy, fxy, wave=wave, mask=mask

  ;set up wavelength-specific parameters
  case wave of
     'NUV' : begin
        ldx     = 10.           ;leeway of pixels to fit around line
        lnterms = 5             ;number of terms in the line fit
        ltol    = 2             ;number of pixels tolerance in result
        lsign   =-1.            ;sign of line amplitude
        line_mask=mask
     end

     'FUV2' : begin
        ldx     = 25.           ;leeway of pixels to fit around line
        lnterms = 5             ;number of terms in the line fit
        ltol    = 5             ;number of pixels tolerance in result
        lsign   = 1.            ;sign of line amplitude
        line_mask=mask
     end

     'FUV1' : begin
        ldx     = 25.           ;leeway of pixels to fit around line
        lnterms = 5             ;number of terms in the line fit
        ltol    = 5             ;number of pixels tolerance in result
        lsign   = 1.            ;sign of line amplitude
        line_mask=mask
     end
  endcase

  ;set a fixed fiducial position and separation
  meandf=539.0  ;average fiducial separation
  y_mid=238.5   ;fix fiducial position

  mindw=0.5     ;minimum width measurable (in pixels)
  
  ;get image dimensions
  nx=(size(img))[1]
  ny=(size(img))[2]

  xarray=dindgen(nx)
  yarray=dindgen(ny)

  if keyword_set(mask) ne 1 then mask=replicate(1,nx,ny)

  ;open an alternate window for plotting results
  window, 0, xsize=500, ysize=300

;------------------------------------------------------------------------------
  ;first get a rough idea of the spectral geometry

  xx=rxy[*,0]
  yy=rxy[*,1]

  rfit=linfit(yy, xx, yfit=xfit)

  plot, yy, xx, psym=4, ystyle=1, xstyle=1, $
        xtitle='Y Pixels', ytitle='X Pixels', $
        title='Rough Spectral Line Geometry'
  oplot, yy, xfit, color=250
  wait, 2

  line_guess=rfit[1]*findgen(ny)

;------------------------------------------------------------------------------
  ;start fitting chosen spectral lines
  nlines = (size(lxy))[1]

  ;temporary buffer for fitted results
  fitted_lines=fltarr(nlines,ny,lnterms)

  lx=lxy[*,0]
  ly=lxy[*,1]

  ;reorder lines left to right (blue to red)
  idxs=sort(lx)
  lx=lx[idxs] & ly=ly[idxs]

  FOR n=0,nlines-1 DO BEGIN
     x0s=lx[n] + line_guess - line_guess[ly[n]]

     ;fit every position along the line in the y direction
     for i=0,ny-1 do begin

        if mask[x0s[i],i] eq 1 then begin
           xx=xarray[x0s[i]-ldx:x0s[i]+ldx] ;x-coordinate array
           ii=img[x0s[i]-ldx:x0s[i]+ldx,i]  ;spectral intensity arrays

           line_mask[xx,i]=1

           fit=gaussfit(xx,ii,a,nterms=lnterms)
;           fit=mpfitpeak(xx, ii, a, nterms=lnterms, /gaussian, /positive)

           if i eq round(ny/2.) then begin
              plot, xx, ii, xstyle=1, ystyle=3, $
                    xtitle='X Pixel', ytitle='Intensity', $
                    title='Spectral Line Fit'
              oplot, xx, fit, color=250
              wait, 1
           endif

           ;throw out absurd fits and populate fit array
           if lsign*a[0] lt 0. or $
              a[1] gt x0s[i]+ltol or $
              a[1] lt x0s[i]-ltol or $
              a[2] lt mindw then a[*]=-!values.f_nan

        endif else a=replicate(-!values.f_nan,lnterms)

        fitted_lines[n,i,*]=a
     endfor 
  ENDFOR

  ;fit array of line centers with a polynomial
  poly_order=2

  lfit=fltarr(nlines,(poly_order+1)*2)

  for i=0,nlines-1 do begin
     xx=fitted_lines[i,*,1]
     idxs=where(finite(xx) eq 1)

     if n_elements(idxs) lt 10 then begin
        print, 'Oops, all the fits for the line at x='+string(lx[i])+' were bad.  Skipping polynomial fits...' 

     endif else begin
        
        yy=yarray[idxs]
        xx=xx[idxs]
        xerr=(fitted_lines[i,*,2])[idxs]
        
        ;fit x in terms of y
        ;use robust_poly_fit to get parameters
        fit=robust_poly_fit(yy,xx,poly_order,yfit)

        ;use regular poly_fit to estimate parameter errors
        tmp=poly_fit(yy,xx,poly_order,sigma=sig, measure_errors=xerr)

        lfit[i,*]=[reform(fit),sig]

        plot, yy, xx, psym=4, ystyle=1, xstyle=1, $
              xtitle='Y Pixel', ytitle='Fitted X Pixel', $
              title='Spectral Geometry Fit'
        oplot, yy, yfit, color=250
        wait, 1.
     endelse
  endfor
  
  ;eliminate bad lines from the results
  good=where(lfit[*,0] ne 0, nlines)
  lfit=lfit[good,*]
  fitted_lines=fitted_lines[good,*,*] ;retroactively remove the gaussian fits

  lines={gaussfit:fitted_lines, polyfit:lfit}

;------------------------------------------------------------------------------
  ;fit chosen fiducials

  ;use a gaussian along entire spectral dimension
  dy     = 20.   ;leeway of pixels to fit around fiducial
  nterms = 6     ;number of terms in fitted gaussian
  tol    = 3     ;number of pixels tolerance in result

  nfiducials=(size(fxy))[1]

  ;temporary buffer for fitted results
  fitted_fiducials=fltarr(nfiducials,nx,nterms)

  fx=fxy[*,0]
  fy=fxy[*,1]

  FOR n=0,nfiducials-1 DO BEGIN
     yy=yarray[fy[n]-dy:fy[n]+dy]

     ;fit every position along the fiducial in the x direction
     for i=0,nx-1 do begin

        if line_mask[i,fy[n]] eq 1 then begin

           ii=img[i,fy[n]-dy:fy[n]+dy]

           ;measure nearby standard deviation
           std=stddev(img[i,fy[n]+dy:fy[n]+2*dy], /nan)

           fit=gaussfit(yy,ii,a,nterms=nterms)
;           fit=mpfitpeak(yy, ii, a, nterms=nterms, /guassian, /negative, /nan)

           if i eq (nx/2) then begin
              plot, yy, ii, xstyle=1, ystyle=3, $
                    xtitle='Y Pixels', ytitle='Intensity', $
                    title='Fiducial Line Fit'
              oplot, yy, fit, color=250
              wait, 1
           endif

          ;throw out absurd fits and populate fit array
           if abs(a[0]) lt 2.*std or $
              a[0] gt 0 or $
              a[2] gt 3. or $
              a[2] lt mindw then a[*]=-!values.f_nan

        endif else a=replicate(-!values.f_nan, nterms)

        fitted_fiducials[n,i,*]=a

     endfor
  ENDFOR

  ;fit array of fiducial centers with a polynomial
  poly_order=1

  ffit=fltarr(nfiducials,(poly_order+1)*2)

  for n=0,nfiducials-1 do begin
     xx=xarray
     yy=fitted_fiducials[n,*,1]
     idxs=where(finite(yy) eq 1)

     xx=xx[idxs]
     yy=yy[idxs]

     fit=robust_poly_fit(xx, yy, poly_order, yfit)
     tmp=poly_fit(xx, yy, poly_order, sigma=sig)

     ffit[n,*]=[reform(fit),sig]

     plot, xx, yy, psym=4, ystyle=1, xstyle=1, $
           xtitle='X Pixel', ytitle='Fitted Y Pixel', $
           title='Fiducial Geometry Fit'
     oplot, xx, yfit, color=250
     wait, 1.
  endfor

  ;collect results for later
  fiducials={gaussfit:fitted_fiducials, polyfit:ffit}

  ;kill window at the end
  wdelete, 0

;------------------------------------------------------------------------------
  ;now carry out analysis of fitted results

  ;decide where the spatial positions along each spectral line should be
  ;for any position on along a spectral line between two fiducials

  ;if there are more than two fiducials selected, only the first two are used?

  ;coordinates of the first (lower) fiducial at line intersections
  c=double(lfit[*,0] + lfit[*,1]*ffit[0,0] + lfit[*,2]*ffit[0,0]^2)
  b=double(lfit[*,1]*ffit[0,1] + 2.*lfit[*,2]*ffit[0,0]*ffit[0,1] - 1.)
  a=double(lfit[*,2]*ffit[0,1]^2)

  x0=(-b - sqrt(b^2-4.*a*c))/(2.*a)
  y0=ffit[0,0] + ffit[0,1]*x0

  ;coordinates of the second (upper) fiducial at line intersections
  c=double(lfit[*,0] + lfit[*,1]*ffit[1,0] + lfit[*,2]*ffit[1,0]^2)
  b=double(lfit[*,1]*ffit[1,1] + 2.*lfit[*,2]*ffit[1,0]*ffit[1,1] - 1.)
  a=double(lfit[*,2]*ffit[1,1]^2)

  x1=(-b - sqrt(b^2-4.*a*c))/(2.*a)
  y1=ffit[1,0] + ffit[1,1]*x1

  ;determine distance between fiducial/line intersection pairs
  df=sqrt((x1-x0)^2 + (y1-y0)^2)
;  meandf=mean(df)
; average fiducial separation is set above to a fixed value
  
  ;reference coordinates for lines and fiducials
  ;position of lower fiducial at middle of array
;  y_mid=ffit[0,0] + ffit[0,1]*float(nx)/2.
; average location of lower fiducial is set above to a fixed value
  
  ;line positions at middle of array
  x_mid=lfit[*,0] + lfit[*,1]*float(ny)/2. + lfit[*,2]*(float(ny)/2.)^2

  for i=0,nlines-1 do begin
     useable=where(finite(fitted_lines[i,*,1]) eq 1, nused)

     ;define line coordinates
     xx=(fitted_lines[i,*,1])[useable]
     yy=yarray[useable]

     ;define coordinate along each line as distance from lower fiducial
     sign=where(yy - y0[i] lt 0)
     rr=sqrt((xx - x0[i])^2 + (yy - y0[i])^2)

     ;scale by distance between lower and upper fiducial
     rr=rr * meandf/df[i]

     tmp=y_mid + rr
     tmp[sign]=y_mid - rr[sign]

     if i eq 0 then begin
        xpos0=replicate(x_mid[i],nused)
        ypos0=tmp
        
        xpos=xx
        ypos=yy
     endif else begin
        xpos0=[xpos0, replicate(x_mid[i],nused)]
        ypos0=[ypos0, tmp]
         
        xpos=[xpos, xx]
        ypos=[ypos, yy]
     endelse
  endfor

;------------------------------------------------------------------------------
  ;now do the surface fitting :D
  ;calculate forward and backward solutions
  polywarp, xpos, ypos, xpos0, ypos0, 2, kx, ky, /DOUBLE
  polywarp, xpos0, ypos0, xpos, ypos, 2, kxi, kyi, /DOUBLE
  
;------------------------------------------------------------------------------
  ;calculate destortion map
  xo=rebin(reform(xarray, nx, 1), nx, ny)
  yo=rebin(reform(yarray, 1, ny), nx, ny)

  xi=replicate(0.D,nx,ny)
  yi=replicate(0.D,nx,ny)

  for i=0,2 do begin
     for j=0,2 do begin
        xi+=kx[i,j] * xo^j * yo^i
        yi+=ky[i,j] * xo^j * yo^i
     endfor
  endfor

;------------------------------------------------------------------------------
RETURN, {kx:kx, ky:ky, kxi:kxi, kyi:kyi, dmapx:xi, dmapy:yi, $
         xpos:xpos, ypos:ypos, xpos0:xpos0, ypos0:ypos0, $
         lines:lines, fiducials:fiducials}

END
