% zonal estimation of surface errors (southwell least square solution)

clear
close all
clc




load slopeRect % load in slope data


SX = reshape(spotshiftxRect,NN,NN);
SX=-SX*(hh1*1000/wavelength)/RR;% x slope
SY = reshape(spotshiftyRect,NN,NN);
SY=-SY*(hh1*1000/wavelength)/RR;% y slope




h=1/floor(NN/2);%speration  between the sampling
NZern=22;

% plot slope data
figure(1)
imagesc(SX);axis equal;colorbar;title('x slope')
figure(2)
imagesc(SY);axis equal;colorbar;title('y slope')

num=size(SX);

starty=1;
mmm1=1:NN;
mmm2=1:NN;
numx=length(mmm2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%x slope matrix
num=size(SX);
t=0;
i=0;
j=0;
for ny=1:NN
    j=j+1;%row
    i=0;
for mx=1:NN
    i=i+1;%colon
    if(mx+1<=num(2))
    t=t+1;
    Sx(t)=(SX(ny,mx+1)+SX(ny,mx))/2;
    Ai1(t)=t;
    Aj1(t)=i+(ny-starty)*numx;
    As1(t)=-1/h;
    Ai2(t)=t;
    Aj2(t)=i+(ny-starty)*numx+1;
    As2(t)=1/h;
    end
end
end
AA1= sparse(Ai1,Aj1,As1,length(mmm1)*(length(mmm2)-1),length(mmm1)*length(mmm2));
AA2= sparse(Ai2,Aj2,As2,length(mmm1)*(length(mmm2)-1),length(mmm1)*length(mmm2));
AA12=AA1+AA2;


%%y slope matrix
 tt=0;
 j=0;
for ny=1:NN
    j=j+1;%row
    i=0;
for mx=1:NN
    i=i+1;%colon
    if(mx+1<=num(2) && ny+1<=num(1))
    tt=tt+1;
    Sy(tt)=-(SY(ny+1,mx)+SY(ny,mx))/2;
    A2i1(tt)=tt;
    A2j1(tt)=i+(ny-starty)*numx;
    A2s1(tt)=1/h;
    A2i2(tt)=tt;
    A2j2(tt)=i+(ny-starty+1)*numx;
    A2s2(tt)=-1/h;
    end
end
end
BB1= sparse(A2i1,A2j1,A2s1,tt,length(mmm1)*length(mmm2));
BB2= sparse(A2i2,A2j2,A2s2,tt,length(mmm1)*length(mmm2));
BB12=BB1+BB2;

A=[AA12;BB12];
S=[Sx';Sy'];
coeff=A\S;
% put phase data to correct position
t=0;
phase1=zeros(size(SX));
for ny=1:NN
for mx=1:NN
    t=t+1;
    phase1(ny,mx)=coeff(t);
end
end
figure;imagesc(phase1);axis equal

%%%%surface fit
num=size(SX);
deta=1;
t=0;
mm=0;
for m=linspace(-1,1,NN)
    mm=mm+1;
    nn=0;
    for n=linspace(-1,1,NN)
        nn=nn+1;
        if(m^2+n^2<=1)
                t=t+1;
                xmirror(t)=n;
                ymirror(t)=m;
                Z(t) = phase1(mm,nn);
        end
         end
end

Asag= PolyFun110321b(xmirror,ymirror,0,NZern);
coeff1=Asag\Z'


% %plot surface
if(1)
t=0;
m=0;
lsf0=coeff1;
lsf0(1:4)=0;
for j=1:-0.02:-1
    m=m+1;
    n=0;
    for i=-1:0.02:1
        n=n+1;
        if (i^2+j^2<=1)
            t=t+1;
            wfa(m,n)=sum(lsf0'*PolyFun110321b(j,i,0,NZern)');
            wfa1(t)=wfa(m,n);
        else
            wfa(m,n)=0;
        end
    end
end
rms=std( wfa1);
figure(101);
imagesc(wfa);colorbar; title ('surface shape')

end
% 
%