;+
; NAME:
;      DSPHASE
;
; PURPOSE:
;      Calculates the phase hologram encoding a desired optical
;      intensity pattern using superposition followed by 
;      direct search refinement.
;
; CATEGORY:
;     Computed holography
;
; CALLING SEQUENCE:
;     phi = dsphase(p)
;
; INPUTS:
;     p: [2,npts] or [3,npts] array of coordinates of traps
;        relative to the center of the focal plane.
;        Coordinates are measured in calibrated pixel units.
;        See CALIBRATE.PRO and HOLO_INIT.PRO for details on calibration.
;
; KEYWORD PARAMETERS:
;     alpha: [npts] array of relative trap intensities.  
;         Default: equal weighting.
;             
;     phase: [npts] array of relative phases of the traps.
;         Default: random numbers in the range [0, 2 pi).
;
;     ell: [npts] array of topological charges for optical vortices.
;         Default: ell = 0 (no vortex).
;             
;     rho: relative weight given to diffraction efficiency versus uniformity
;         0: diffraction efficiency
;         1: uniformity
;         Default: 0.5
;
;     eps: Convergence threshold.
;         Default: 5e-5
;           
;     nlevels: Number of phase levels per pixel.
;         Default: 256 (8 bits)
;
;     npasses: Number of passes through the phase mask.
;         Default: 1.
;             
;     inphi: Input phase pattern to be improved.
;         Default: Start from superposition of fields with random
;                  relative phases.
;
; KEYWORD FLAGS:
;     windowon: turn on compensation for windowing effect    	
;
;     interval: Calculate diagnostics after this many trial updates.
;         Default: 1000, or 10000 if SLM is set.
;
;     slm: If set, then project present hologram every INTERVAL
;         iterations.  INTERVAL should be at least 10000 to avoid
;         spinning the SLM too badly.
;
;     fake: Pass FAKE keyword to SLM.
;
;     quiet: Suppress diagnostics, if set.
;
;     realquiet: Say *nothing*, if set.
;      
; OUTPUTS:
;      phi: phase pattern encoding pattern of traps described by
;          P, ALPHA, and ELL, with values ranging from [0,2 pi].
;
; RESTRICTIONS:
;      Can be very memory intensive for large numbers of points.
;
; PROCEDURE:
;      Initial estimate is obtained by superposing the fields of the
;      specified beams.  The resulting phase pattern is improved by
;      randomly changing pixels and keeping the changes that improve
;      the convergence factor defined by
;
;      Martin Meister and Richard J. Winfield, Opt. Commun. 203, 39-49 (2002).
;
; NOTES:
;      If we calibrate the SLM's phase transfer function, then we
;      should be able to pass the lookup table to this function, and
;      return a hologram of indices into the look-up table.
;
; MODIFICATION HISTORY:
; Created by David G. Grier, New York University, 5/24/2004.
; 05/25/2004: DGG. Added comments, implemented NPASSES, and cleaned up
;      code.
; 05/27/2004: DGG. Fixed relative weighting, corrected efficiency 
;      calculation, improved reporting.
; 06/01/2004: DGG. Fixed weighting for "zero" amplitude traps.
;      Added RHO keyword.  Added QUIET keyword. Added INFO output keyword.
; 06/18/2004: DGG. Really fixed weighting for "zero" amplitude traps.
; 07/25/2004: Sang-Hyuk Lee, NYU.  Added WINDOWON keyword to
;      compensate Nyquist windowing effect.
; 01/29/2005: DGG. Small code cleanups.
; 01/31/2005: DGG. Added INTERVAL keyword diagnostic reporting.
;      Fixed a couple of quantization bugs leading to faster convergence.
; 03/11/2005: DGG. Put in LUT hooks to specify phase look-up-table
;      based on a (measured) SLM phase transfer function.
;      Need to think of a convenient way to do the inverse
;      look-up to quantize the initial phase estimate.
; 07/21/2006: DGG. Added RELPHASE keyword.
; 10/13/2006: DGG. Compute phase with ATAN(psi, /PHASE).
; 01/03/2010: DGG. Do not clobber input parameters.  Code cleanup.
;      Documentation cleanup.  Removed INFO keyword.
; 01/04/2010: DGG. Cleaned up alpha code, including a bug fix
;      when using WINDOWON.  Cleaned up reporting code.
; 01/05/2010: DGG. Fixed NLEVELS code so that phase levels
;      go from 0 to 2 pi (NLEVELS-1)/NLEVELS.  This improves
;      behavior for small values of NLEVELS.  Documentation
;      improvements.
; 06/10/2010: DGG. Added COMPILE_OPT
;
; Copyright 2006-2010 David G. Grier and Sang-Hyuk Lee
;
; UPDATES:
;    The most recent version of this program may be obtained from
;    http://physics.nyu.edu/grierlab/software.html
; 
; LICENSE:
;    This program is free software; you can redistribute it and/or
;    modify it under the terms of the GNU General Public License as
;    published by the Free Software Foundation; either version 2 of the
;    License, or (at your option) any later version.
;
;    This program is distributed in the hope that it will be useful,
;    but WITHOUT ANY WARRANTY; without even the implied warranty of
;    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
;    General Public License for more details.
;
;    You should have received a copy of the GNU General Public License
;    along with this program; if not, write to the Free Software
;    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
;    02111-1307 USA
;
;    If the Internet and WWW are still functional when you are using
;    this, you should be able to access the GPL here: 
;    http://www.gnu.org/copyleft/gpl.html
;-

; Traps near the Nyquist boundary are suppressed.  Compensate for this
; by pre-emphasizing them.
function dsphase_sinc, x

COMPILE_OPT IDL2, HIDDEN

 w = where(abs(x) gt 1.e-5, ngood)
s = 0.D * x + 1.D
if ngood gt 0 then $
   s[w] = sin(x[w])/x[w]
return, s
end

function dsphase, p, $                 ; coordinates of traps
                  alpha = alpha_,  $   ; relative intensities
                  phase = phase_, $    ; relative phases
                  ell = ell_, $        ; topological charges
                  rho = rho, $         ; weighting for error calculation
                  nlevels = nlevels, $ ; number of phase levels
                  npasses = npasses, $ ; maximum number of passes through array
                  eps = eps, $         ; error tolerance for convergence test
                  inphi = inphi, $     ; input phase estimate
                  windowon = windowon, $ ; correct for Nyquist windowing
                  quiet = quiet, realquiet = realquiet, $ ; limit reporting
                  interval = interval_, $ ; reporting interval
                  slm = slm, fake = fake  ; project at INTERVAL iterations

COMPILE_OPT IDL2

common holo_common, cal

; phase mask dimensions
if n_elements(inphi) gt 0 then begin
   phi = inphi
   sz = size(inphi)
   w = double(sz[1])
   h = double(sz[2])
endif else begin
   w = cal.doe_w
   h = cal.doe_h
endelse
maskarea = double(w) * double(h)

twopi = 2.D * !dpi

; calibration constants
; center of phase mask relative to center of SLM.
xc = cal.xc
yc = cal.yc
; scale factors for square trapping geometry oriented with camera.
xfac = cal.xscale
rfac = cal.scale
thetac = cal.theta
zfac = cal.zfactor

; number of phase levels
if n_elements(nlevels) lt 1 then $
   nlevels = 256
if nlevels lt 2 then $
   message, "Hologram must have at least two phase levels: NLEVELS > 1"

if n_elements(npasses) lt 1 then $
   npasses = 1
if nlevels lt 1 then $
   message, "At least one pass is required: NPASSES > 0"

if n_elements(eps) lt 1 then $
   eps = 5.D-5

; relative weighting between efficiency (rho = 0) and
; uniformity (rho = 1)
if n_elements(rho) eq 1 then $
   rho = double(rho) < 1. > 0. $
else $
   rho = 0.5D

; interval at which to calculate and report diagnostics
if n_elements(interval_) eq 1 then $
   interval = abs(long(interval_)) $
else $
   interval = 1000L

doslm = keyword_set(slm)
if doslm and interval lt 10000L then $
   interval = 10000L
fake = keyword_set(fake)

talkative = not keyword_set(realquiet)
chatty = talkative and not keyword_set(quiet)
if talkative then fmt = '(I12,3F12.6)'

; trap locations in trapping plane
sp = size(p)
ndim = sp[1]
if ndim ne 2 and ndim ne 3 then $
   message, "requires 2- or 3-dimensional trap coordinates"
npts = sp[2]

; relative intensities
if n_elements(alpha_) eq npts then $
   alpha = double(alpha_) $
else $
   alpha = replicate(1.D, npts)

; account for zero-intensity traps
nonzero = where(alpha gt 1.d-5, ngood)
if ngood eq 0 then begin
   message, "no finite-intensity points specified"
   return, fltarr(w,h)
endif
if ngood ne npts then $
   alpha = alpha[nonzero]

; rotate points to account for relative SLM-CCD orientation
kx = (twopi/w) * (p[0, nonzero] * cos(thetac) + p[1, nonzero] * sin(thetac))
ky = (twopi/h) * (p[1, nonzero] * cos(thetac) - p[0, nonzero] * sin(thetac))
if ndim gt 2 then kz = twopi * zfac * p[2, nonzero]

; adjust relative intensities for windowing
if keyword_set(windowon) then begin
   if talkative then message, "Compensating windowing effect ...", /inf
   win = dsphase_sinc(0.5 * kx) * dsphase_sinc(0.5 * ky) 
   alpha /= (win^2 > 0.01)
endif

; normalization for error estimates
alpha /= total(alpha)           ; intensity normalization

; relative phase factors
if n_elements(phase_) eq npts then $
   phase = exp(dcomplex(0, phase_[nonzero])) $
else $
   phase = exp(dcomplex(0, twopi * randomu(seed, ngood)))

; optical vortices
dovortex = n_elements(ell_) eq npts
if dovortex then $
   ell = ell_[nonzero]

; coordinates in SLM plane (row vectors)
x = xfac * rfac * (dindgen(w) - w/2.D)
y = rfac * (dindgen(h) - h/2.D)
if ndim gt 2 then $
   irsq = dcomplex(0, $
                   (x - xc)^2 # replicate(1.D, h) + $
                   replicate(1.D, w) # (y - yc)^2)
if dovortex then $
   itheta = dcomplex(0., $
                     atan(replicate(1.D, w) # (y - yc), $
                          (x - xc) # replicate(1.D, h)))

; forward propagators (row vectors)
; NOTE: each propagator consists of
;     MASKAREA * NPTS double-precision complex numbers.
;     This can be extremely memory intensive for large arrays.
ex = exp(dcomplex(0, -x) # kx)
ey = exp(dcomplex(0, -y) # ky)

; initial estimate for phase (superposition)
if n_elements(inphi) eq 0 then begin
   if talkative then $
      message, "Calculating initial phase estimate by superposition ...", /inf
   psi = dcomplexarr(w, h)
   for n = 0, ngood-1 do begin
      tpsi = sqrt(alpha[n]) * phase[n] * conj(ex[*, n] # ey[*, n])
      if ndim gt 2 then tpsi *= exp(-irsq * kz[n])
      if dovortex then tpsi *= exp(-itheta * ell[n])
      psi += tpsi
   endfor
   ephi = psi / abs(psi)
   phi = atan(psi, /phase) + !dpi
endif else begin
   phi = inphi
   ephi = exp(dcomplex(0., phi))
endelse

; quantize phase
fac = twopi / nlevels
nphi = fix(phi / fac) <  (nlevels - 1L)

; phase per pixel look-up table
lut = fac * dindgen(nlevels)
phaselut = exp(dcomplex(0., lut)) / maskarea

; initial wavefunction at trap locations
if talkative then $
   message, "Calculating wavefunction at trap locations ...", /inf

psi = dcomplexarr(1, ngood)
for n = 0, ngood-1 do begin
   phase = ex[*, n] # ey[*, n]
   if ndim gt 2 then $
      phase *= exp(irsq * kz[n])
   if dovortex then $
      phase *= exp(itheta * ell[n])
   psi[n] = total(phaselut[nphi] * phase)
endfor

; intensity at trap locations
inten = double(psi * conj(psi))

meaninten = mean(inten)
gamma = total(alpha * inten)
sigma = sqrt(mean((inten - gamma * alpha)^2))
con = rho * sigma - (1.D - rho) * meaninten

efficiency = total(inten)
rmserror = sigma / max(inten)
maxi = max(inten / alpha, min = mini)
uniformity = (maxi - mini) / (maxi + mini)

if talkative then begin
   eff0 = efficiency
   rms0 = rmserror
   uni0 = uniformity
   message, "Calculating CGH by direct search ...", /inf
endif
if chatty then $
   print,  "Changes",  "Efficiency",  "Error",  "Uniformity", $
           format = '(4A12)'

naccept = 0L
for pass = 1, npasses do begin
    ndx = long(randomu(seed, maskarea) * maskarea)
    ocon = con
    for m = 0L, maskarea - 1L do begin
       n = ndx[m]               ; index of this pixel
       nx = n mod w             ; index of x-coordinate
       ny = fix(n / w)          ; index of y-coordinate
       
    ; Try replacing the value at this pixel with any of the other 
    ; possible values by randomly shifting the phaselut index at this pixel
       tnphi = (nphi[n] + long(randomu(seed, 1) * (nlevels-1))) mod nlevels
    
    ; Calculate the phase shift associated with propagating from this 
    ; DOE pixel into the desired mode at the desired position:
       phase = ex[nx, *] * ey[ny, *]
       if ndim gt 2 then phase *= exp(irsq[n] * kz)
       if dovortex then phase *= exp(itheta[n] * ell)
 
    ; Update the field at each of the specified positions by subtracting
    ; the existing DOE pixel's contribution and replacing it with the
    ; new trial value.
       tpsi = psi + (phaselut[tnphi]  - phaselut[nphi[n]])[0] * phase
       tinten = double(tpsi * conj(tpsi)) ; the associated intensities

    ; Calculate convergence factor:
       meaninten =  mean(tinten)
       gamma = total(alpha * tinten) / total(alpha^2)
       tsigma = sqrt(mean((tinten - gamma * alpha)^2))
       con = rho * tsigma - (1.d - rho) * meaninten

    ; Accept the new pixel value if convergence factor has improved
       if con lt ocon then begin
          nphi[n] = tnphi       ; Use the new DOE pixel value ...
          psi = tpsi            ; ... and update the trap fields.
          ocon = con            ; Use the new convergence factor.
          naccept++             ; Take note that a change was accepted.
          inten = tinten        ; update factors only when a move is accepted
          sigma = tsigma        ; to avoid having numbers jump around
       endif

    ; Feedback for the user.
       if m mod interval eq 0 then begin
          efficiency = total(inten)
          rmserror = sigma / max(inten)
          maxi = max(inten / alpha, min = mini)
          uniformity = (maxi - mini) / (maxi + mini)
          if chatty then $
             print, naccept, efficiency, rmserror, 1. - uniformity, format = fmt
          if keyword_set(slm) then $
             slm, fac * nphi, fake = fake
          if (rmserror lt eps) and (uniformity lt eps) then $
             goto, done
       endif
    endfor
 endfor

done:

; Final statistics regarding the DOE's performance.
if chatty then print
if talkative then begin
   efficiency = total(inten)
   rmserror = sigma / max(inten)
   maxi = max(inten / alpha, min = mini)
   uniformity = (maxi - mini) / (maxi + mini)
   print, "Changes", "Efficiency", "Error", "Uniformity", format = '(4A12)'
   print, 0, eff0, rms0, 1. - uni0, format = fmt
   print, naccept, efficiency, rmserror, 1. - uniformity, format = fmt
endif
    
return, lut[nphi]
end