%%?initialization
close?all;
clear;
clc;
%%?parameters
lamuda=0.632;%波長?um
D_led=4*1000;%LED間距
h_err=86*1000;%LED與樣品間距（校正前）
ledMM=15;ledNN=15;%15X15的LED陣列
ledM=ledMM;ledN=ledNN;
k_lamuda=2*pi/lamuda;%波數

pixel_size=6.5;%相機像素尺寸
mag=4;%顯微鏡放大倍率
NA=0.1;%物鏡數值孔徑
M=128;N=128;%拍攝到的圖像大小
D_pixel=pixel_size/mag;%像面像素尺寸
kmax=NA*k_lamuda;%物鏡數值孔徑對應的最大波數半徑

%重構后圖像相比原始圖像放大倍率?1+2*D_pixel*sin（theta）/lamda?ceil向上取整?單個維度!!
MAGimg=ceil（1+2*D_pixel*7*D_led/sqrt（（7*D_led）^2+h_err^2）/lamuda）;%重構后圖像相比原始圖像放大倍率
MM=M*MAGimg;NN=N*MAGimg;%重構后圖像大小

Niter0=10;%初始迭代次數
Niter1=50;%225幅圖像迭代次數
Cuv=8;%模擬退火法的初始搜索步長

%%?misalignment
objdx=835.7472;%樣品中所選的小區(qū)域的位置
objdy=835.7472;
theta=5;%*（2*rand（1）-1）;%LED板的旋轉誤差
leddx=1000;%*（2*rand（1）-1）;%LED板的橫向位置誤差
leddy=1000;%*（2*rand（1）-1）;
h=h_err+1000;%*（2*rand（1）-1）;%LED板和樣品的真實間距
disp（‘iter?theta??led_dx??led_dy???h????????分別為‘）;
disp（vpa（[0thetaleddxleddyh]4））;

%%?object
I0=im2double（imread（‘I0.bmp‘））;
P0=im2double（imread（‘P0.bmp‘））;
I0=imresize（I0[MMNN]）;%物體的真實強度
P0=imresize（P0[MMNN]）;%物體的真實相位
o1=sqrt（I0）.*exp（sqrt（-1）.*P0）;%物體的復振幅
O1=fftshift（fft2（o1））;

%%?頻域坐標

[Fx1Fy1]=meshgrid（-（N/2）:（N/2-1）-（M/2）:（M/2-1））;
Fx1=Fx1./（N*D_pixel）.*（2*pi）;%原始圖像的頻域坐標
Fy1=Fy1./（M*D_pixel）.*（2*pi）;%原始圖像的頻域坐標
Fx2=Fx1.*Fx1;
Fy2=Fy1.*Fy1;
Fxy2=Fx2+Fy2;
Pupil0=zeros（MN）;
Pupil0（Fxy2<=（kmax^2））=1;%物鏡在頻域的孔徑
[Fxx1Fyy1]=meshgrid（-（NN/2）:（NN/2-1）-（MM/2）:（MM/2-1））;
Fxx1=Fxx1（1:）./（N*D_pixel）.*（2*pi）;%重構圖像的頻域坐標
Fyy1=Fyy1（:1）./（M*D_pixel）.*（2*pi）;%重構圖像的頻域坐標
?
mesh（Pupil0）

%%?每個LED在頻域對應的像素坐標
ledpos_N=2;
ledpos_err=zeros（ledMMledNN2）;
ledpos_true=zeros（ledMMledNN2）;
ledpos_sum=zeros（2ledMM*ledNNledpos_N）;
k=zeros（ledMMledNN）;
for?i=1:ledMM
????for?j=1:ledNN
????????k（ij）=j+（i-1）*ledNN;
????????
????????m=i-（ledMM+1）/2;
????????n=j-（ledNN+1）/2;
????????x=objdx-（m*D_led）;
????????y=objdy-（n*D_led）;
????????u=k_lamuda*x/sqrt（x^2+y^2+h_err^2）;
????????v=k_lamuda*y/sqrt（x^2+y^2+h_err^2）;
????????Fx1_temp=abs（Fxx1-u.*ones（1NN））;
????????ledpos_err（ij1）=find（Fx1_temp==min（Fx1_temp））;
????????Fy1_temp=abs（Fyy1-v.*ones（MM1））;
????????ledpos_err（ij2）=find（Fy1_temp==min（Fy1_temp））;%不知道LED板有誤差時的每個LED在頻域對應的像素坐標
????????
????????m=[（i-（ledMM+1）/2）（j-（ledNN+1）/2）]*[cos（theta*pi/180）;sin（theta*pi/180）];%%%%
????????n=[（i-（ledMM+1）/2）（j-（ledNN+1）/2）]*[-sin（theta*pi/180）;cos（theta*pi/180）];%%%%%%
????????x=objdx-（m*D_led+leddx）;%%%
????????y=objdy-（n*D_led+leddy）;%%%
????????u=k_lamuda*x/sqrt（x^2+y^2+h^2）;
????????v=k_lamuda*y/sqrt（x^2+y^2+h^2）;
????????Fx1_temp=abs（Fxx1-u.*ones（1NN））;
????????ledpos_true（ij1）=find（Fx1_temp==min（Fx1_temp））;
????????Fy1_temp=abs（Fyy1-v.*ones（MM1））;
????????ledpos_true（ij2）=find（Fy1_temp==min（Fy1_temp））;%已知LED板的誤差時的每個LED在頻域對應的像素坐標
????????
????????ledpos_sum（1k（ij）1）=ledpos_err（ij1）;
????????ledpos_sum（2k（ij）1）=ledpos_err（ij2）;
????????ledpos_sum（1k（ij）2）=ledpos_true（ij1）;
????????ledpos_sum（2k（ij）2）=ledpos_tr