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

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, alpha = alpha, ell = ell, relphase = relphase, $
                  windowon = windowon, $
                  rho = rho, nlevels = nlevels, inphi = inphi, $
                  npasses = npasses, eps = eps, $
                  info = info, quiet = quiet, realquiet = realquiet, $
		  interval = interval, slm = slm, fake = fake
;+
; 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] array of (x,y) coordinates of points in the plane,
;         relative to the center of the field of view.
;         Coordinates are measured in calibrated pixel units.
;         See CALIBRATE.PRO and HOLO_INIT.PRO for details on calibration.
;
; OPTIONAL INPUTS:
;      p: [3,npts] array of (x,y,z) coordinates of points in the focal
;         volume, relative to the center of the focal plane.
;         Coordinates are measured in pixel units.
;
; KEYWORD PARAMETERS:
;      alpha: [npts] array of relative trap intensities.  
;             Default: uniform weighting of 1 per trap.
;             
;      ell: [npts] array of topological charges for optical vortices.
;             Default: ell = 0 (no vortex).
;             
;      relphase: [npts] array of relative phases of the traps.
;             Default: random numbers in the range [0, 2 pi).
;
;      rho: relative weight given to diffraction efficiency versus uniformity
;           0: diffraction efficiency
;           1: uniformity
;           Default: 0.5
;
;      eps: Quit when the errors reach this value.
;           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.
;
;      interval: Calculate diagnostics after this many trial updates.
;             Default: 1000.
;             Defaults to 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.
;
;      quiet: Suppress diagnostics, if set.
;      realquiet: Say *nothing*.
;
;      windowon: turn on/off the compensating of windowing effect    	
;      
; OUTPUTS:
;      phi: phase pattern encoding pattern of traps described by
;           P, ALPHA, and ELL, with values ranging from [0,2 pi].
;
; OPTIONAL OUTPUT KEYWORD:
;      info: convergence history: 
;            [naccepted, efficiency, nonuniformity, rms error]
;
; 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.
; 5/25/2004: DGG.  Added comments, implemented NPASSES, and cleaned up
;                  code.
; 5/27/2004: DGG.  Fixed relative weighting, corrected efficiency 
;                  calculation, improved reporting.
;  6/1/2004: DGG.  Fixed weighting for "zero" amplitude traps.
;                  Added RHO keyword.  Added QUIET keyword.
;                  Added INFO output keyword.
; 6/18/2004: DGG.  Really fixed weighting for "zero" amplitude traps.
; 7/25/2004: Sang-Hyuk Lee, NYU.  Added weighting factor for "alpha" to compensate 
;                  the windowing effect of a pixel of SLM.
; 1/29/2005: DGG.  Small code cleanups.
; 1/31/2005: DGG.  Added INTERVAL keyword diagnostic reporting.
;                  Fixed a couple of quantization bugs leading to
;                  faster convergence.
; 3/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.
;  7/21/2006: DGG. Added RELPHASE keyword.
; 10/13/2006: DGG. Compute phase with ATAN(psi, /PHASE).
;
; Copyright (c) 2006 David G. Grier and Sang-Hyuk Lee
; 
; 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 shold be able to access the GPL here: 
;    http://www.gnu.org/copyleft/gpl.html
;-

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 n_elements(npasses) lt 1 then npasses = 1

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(interval) $
else $
  interval = 1000

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

talkative = not keyword_set(realquiet)
chatty = not keyword_set(quiet) and not keyword_set(realquiet)

; trap locations in trapping plane
sp = size(p)
npts = sp[2]
ndim = sp[1]

q = p
; rotate points to account for relative SLM-CCD orientation
q[0, *] = p[0, *] * cos(thetac) + p[1, *] * sin(thetac)
q[1, *] = p[1, *] * cos(thetac) - p[0, *] * sin(thetac)
kx = (twopi/w) * double(q[0, *])
ky = (twopi/h) * double(q[1, *])

if ndim gt 2 then $
    kz = twopi * zfac * p[2, *]

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

alp = alpha

; compensate windowing effect
if keyword_set(windowon) then begin
      message, "Compensating windowing effect ...", /inf
  win = dsphase_sinc(!dpi/w*q[0,*])*dsphase_sinc(!dpi/h*q[1,*]) 
  alp /= (win^2 > 0.01)
endif

alp = alp/total(alp)
nonzero = where(alp gt 1.d-5)

; optical vortices
dovortex = n_elements(ell) eq npts

; 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)
ex = exp(dcomplex(0, -1) * x # kx)
ey = exp(dcomplex(0, -1) * 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
    if n_elements(relphase) eq npts then $
	    phase = exp(dcomplex(0,relphase)) $
    else $
    	phase = exp(dcomplex(0, twopi * randomu(seed, npts)))
    psi = dcomplexarr(w, h)
    for n = 0, npts-1 do begin
        tpsi = sqrt(alp[n]) * phase[n] / (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
nphi = fix(phi / twopi * (nlevels-1.D)) <  (nlevels-1L)

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

; initial wavefunction at trap locations
if talkative then $
  message, "Calculating wavefunction at trap locations ...", /inf
psi = dcomplexarr(1, npts)
for n = 0, npts-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(alp*inten)/total(alp^2)
sigma = sqrt(mean((inten-gamma*alp)^2))
con = rho * sigma - (1.D - rho) * meaninten

efficiency = total(inten)/total(alp)
rmserror = sigma/max(inten)
maxi = max(inten[nonzero]/alp[nonzero], min = mini)
uniformity = (maxi - mini)/(maxi + mini)

info = [0., efficiency, rmserror, uniformity]
if chatty then $
  print, con, efficiency, rmserror, uniformity

if talkative then $
  message, "Calculating CGH by direct search ...", /inf
for pass = 1, npasses do begin
    ndx = long(randomu(seed, maskarea) * maskarea)
    ocon = con
    naccept = 0.
    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(alp*tinten)/total(alp^2)
        tsigma = sqrt(mean((tinten-gamma*alp)^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 += 1.       ; 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)/total(alp)
            rmserror = sigma/max(inten)
            maxi = max(inten[nonzero]/alp[nonzero], min = mini)
            uniformity = (maxi-mini)/(maxi+mini)
            info = [[info], [naccept, efficiency, rmserror, uniformity]]
            if chatty then $
              print, m, naccept, efficiency, rmserror, uniformity
            if keyword_set(slm) then $
              slm, nphi, /raw, fake=fake
            if (rmserror lt eps) and (uniformity lt eps) then $
              goto, done
        endif
    endfor
endfor

done:
; Final statistics regarding the DOE's performance.
efficiency = total(inten)/total(alp)
rmserror = sigma/max(inten)
maxi = max(inten[nonzero]/alp[nonzero], min = mini)
uniformity = (maxi-mini)/(maxi+mini)
if talkative then $
  print, naccept, efficiency, rmserror, uniformity
    
return, lut[nphi]
end