% Demodulate image array (im) of Hartmann-Shack spots at known pitch (pitch) 
% Outputs: idx, idy are complex arrays whose phases are the x and y gradients
% Upon first run (li==0) perform demodulation and save reference arrays iex, iey
% 
% Each image is first multiplied by phasors of the pitch frequency, then smoothed
% at 1/2, 1/4, 1/8,,, pitch (usualy it's possible to comment out the 1/8 steps). 
% Smoothing is performed by shifting the array both ways in pitch fractions and 
% summing with the original array, then repeating in orthogonal direction.

% Erez Ribak 2004

close all; clear all;
for li = 0:1   
    fup = ['a' int2str(li) '.bmp'] % file names: a0.bmp (reference), a1.bmp
    im = double (imread (fup)); % read file
    if li == 0,                                 % li=0 if reference image, 1 all other images
        pitch = 36.12;                      %  pixels between HS spots, in this example equal in x and y
        rp2 = round (pitch/2); rp4 = round (pitch/4); rp8 = round (pitch/8); % smoothing step sizes
        samplegrid = round (sr/pitch - [1 1]);   % # of points to sample
        iex = 1; iey = 1;                                            % reference phasor arrays, initially 1
        sr = size (im);                                               % size of input image array of HS spots
        i = sqrt (-1); pi = acos (-1);                      % just in case
        rulex = -i * (1:sr(2)) * 2 * pi / pitch;     % linear phase along x
        ruley = -i * (1:sr(1)).' * 2 * pi / pitch;   % ditto along y
    end
    
    idx = im .* (ones (sr(1),1) * exp (rulex));   % add complex phase to image along x
    idx = 2 * idx + circshift (idx, [rp2 0]) + circshift (idx, [-rp2 0]); % smooth at half pitch, y
    idx = 2 * idx + circshift (idx, [rp4 0]) + circshift (idx, [-rp4 0]); % smooth at quarter pitch
    idx = 2 * idx + circshift (idx, [rp8 0]) + circshift (idx, [-rp8 0]); % smooth at one eighth pitch
    idx = 2 * idx + circshift (idx, [0 rp2]) + circshift (idx, [0 -rp2]); % smooth at half pitch, x
    idx = 2 * idx + circshift (idx, [0 rp4]) + circshift (idx, [0 -rp4]); % smooth at quarter pitch
    idx = 2 * idx + circshift (idx, [0 rp8]) + circshift (idx, [0 -rp8]); % smooth at one eighth pitch
    idx = idx .* iex;                                                                                                 % remove reference phase
    if li==0 iex = conj (idx); end                      % save reference phase array, x gradient
    
    idy = im .* (exp (ruley) * ones (1, sr(2)));    % add complex phase to image along y
    idy = 2 * idy + circshift (idy, [rp2 0]) + circshift (idy, [-rp2 0]); % smooth at half pitch, y
    idy = 2 * idy + circshift (idy, [rp4 0]) + circshift (idy, [-rp4 0]); % smooth at quarter pitch
    idy = 2 * idy + circshift (idy, [rp8 0]) + circshift (idy, [-rp8 0]); % smooth at one eighth pitch
    idy = 2 * idy + circshift (idy, [0 rp2]) + circshift (idy, [0 -rp2]); % smooth at half pitch, x
    idy = 2 * idy + circshift (idy, [0 rp4]) + circshift (idy, [0 -rp4]); % smooth at quarter pitch
    idy = 2 * idy + circshift (idy, [0 rp8]) + circshift (idy, [0 -rp8]); % smooth at one eighth pitch
    idy = idy .* iey;                                                                                                 % remove reference phase
    if li==0 iey = conj (idy); end                        % save reference phase array, y gradient
    
    tempx = imresize(idx(rp2:end-rp2+1,rp2:end-rp2+1),samplegrid); % sample points
    tempy = imresize(idy(rp2:end-rp2+1,rp2:end-rp2+1),samplegrid); % sample points
    a = [tempx(:)' tempy(:)'];                                        % x, y gradients into linear array
    
    figure; imagesc (angle (idx)); colorbar
    figure; imagesc (angle (idy)); colorbar
    drawnow
end