% Clear the memory, close all figures and clear the screen
clear all
close all
clc

%bw compatibility
vision.setCoordinateSystem('RC') 

%constants for the program
L = 2/0.005;

% set up arrays and plot some simple figures
[x y] = meshgrid(-1:0.005:1,-1:0.005:1);
r=sqrt(x.^2+y.^2);
theta=atan2(y,x);
circ = (r<1);
coma=2*sqrt(2)*(3*r.^3-2*r).*cos(theta).*circ;
figure(1);
imshow(coma,[]);
figure(3);
intens=abs(exp(1i*(30*x+40*y))-circ.*exp(1i*coma)).^2;
imshow(intens,[]);
% create a random image with low spatial frequency content
ri = rand( [50 50])-0.5;
kern = imresize(exp(-r.^2/(2*0.25^2)),[30 30]);
ri = conv2(ri,kern);
ri = imresize(ri,20,'bicubic');
ri = ri(300:700,300:700).*circ;
figure(20);
imshow(ri,[]);
% what is RMS aberration and how much coma is in it?
rms = sqrt(sum(ri(:).^2)/sum(circ(:)))
c = sum(ri(:).*coma(:))/sum(circ(:))
% create a test wavefront
blank = zeros(size(coma));
%textInserter = vision.TextInserter('MSc Lab','Color', 0.1,'FontSize', ...
%72, 'Location', [150 60]);
%textim = step(textInserter,blank);
testphase = coma ;%+ textim;
testwf = circ.*exp(1i*testphase);

% 3 step PSI algorithm
I1=abs(exp(1i*(30*x+40*y))+testwf).^2;
I2=abs(exp(1i*(30*x+40*y+2*pi/3))+testwf).^2;
I3=abs(exp(1i*(30*x+40*y+4*pi/3))+testwf).^2;
p=circ.*atan2(sqrt(3)/2*(I2-I3),(I1-(I2+I3)/2));
figure(5);
imshow(p,[]);

% 4 stepa PSI algorithm
tilt = (30*x+40*y);
I1=abs(exp(1i*tilt)+testwf).^2;
I2=abs(exp(1i*(tilt+pi/2))+testwf).^2;
I3=abs(exp(1i*(tilt+pi))+testwf).^2;
I4 = abs(exp(1i*(tilt+3*pi/2))+testwf).^2;
p=circ.*atan2((I2-I4),(I1-I3));
figure(5);
imshow(p,[]);

% 5-step algorithm
step = 2*pi/5;

I1=abs(exp(1i*(30*x+40*y+step))+testwf).^2;
I2=abs(exp(1i*(30*x+40*y+2*step))+testwf).^2;
I3=abs(exp(1i*(30*x+40*y+3*step))+testwf).^2;
I4=abs(exp(1i*(30*x+40*y+4*step))+testwf).^2;
I5=abs(exp(1i*(30*x+40*y))+testwf).^2;
m1 = sqrt(2*(5+sqrt(5)));
m2 = sqrt(2*(5-sqrt(5)));
n2 = 1 + sqrt(5);
n1 = 1 - sqrt(5);
p=circ.*atan2(m1*(I1-I4)+m2*(I2-I3) , n1*(I1+I4)+n2*(I2+I3)-4*I5);
figure(5);
imshow(p,[]);

% Hariharan algorithm
step = pi/2;

I1=abs(exp(1i*(30*x+40*y-2*step))+testwf).^2;
I2=abs(exp(1i*(30*x+40*y-step))+testwf).^2;
I3=abs(exp(1i*(30*x+40*y))+testwf).^2;
I4=abs(exp(1i*(30*x+40*y+step))+testwf).^2;
I5=abs(exp(1i*(30*x+40*y+2*step))+testwf).^2;
p=circ.*atan2(2*(I2-I4),(2*I3-I5-I1));
figure(6);
imshow(p,[]);

% Fourier algorithm
% We introduce a spatial frequency
f0 = 25;
tilt = (2*pi*f0*x);
I=abs(exp(1i*tilt)+testwf).^2;
imshow(I,[]);
Fs = 200;
NFFT = 2^nextpow2(401); % Next power of 2 from length of y
Y = fftshift(fft(I,NFFT,2));
%we filter out around f0
mask = zeros(401,NFFT);
range = floor(NFFT*(.5-f0/Fs)) + (floor(-NFFT/25):ceil(NFFT/25)); 
mask(:,range) = 1;
Y_filter = circshift(Y.*mask,[0 floor(f0*NFFT/Fs)]);
imshow(abs(Y_filter),[]);
phase = ifft(fftshift(Y_filter),NFFT,2);
c = phase(:,1:401);
p=circ.*angle(c);
figure(5);
imshow(p,[]);

% try to unwrap the phase
figure(6);
imshow(unwrap(unwrap(p,[],2),[],1),[]);
% try to unwrap phase from middle out half of image at a time
q=p;
q(200:end,:)=unwrap(p(200:end,:),[],1);
q=flipud(q);
q(200:end,:)=unwrap(q(200:end,:),[],1);
q(:,200:end)=unwrap(q(:,200:end),[],2);
q=fliplr(q);
q(:,200:end)=unwrap(q(:,200:end),[],2);
q=flipud(fliplr(q));
figure(70);
imshow(q,[]);
% take off tilt to see what is left
figure(8);
q=circ.*(q+30*x+40*y);
imshow(q,[]);
% take off aberration to see what is left
figure(9);
c = sum(q(:).*coma(:))/sum(circ(:));
imshow(q-c*coma,[]);



