% Peng Su 09/24/09

%polynomial fitting method for reducing wavefront slope data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
close all;
clc
clear


for mon=1:1

    NN=9;% 9by9 sampling 
    wavelength=0.633;%um
    RR=10;% focal length of a single lenslet
    hh=5/2;%semi-diameter of the lenslets array
    hh1=hh/NN;%semi-diameter of a single lenslet
    NZern =22;%Number of Zernike Polys used
    coeff0=zeros(NZern,1);% intialize wavefront coefficents
    coeff0(7)=1; % aberration added in wave unit
%     coeff0(11)=0;% aberration added in wave unit
    
    plotrawdata=1;% plot data
    plotmap=1;%plot reconstructed map
    % during Montel Carlo simulation, you would like to set plotrawdata and
    % plotmap to be zero to save computing time

      % generate  wavefront sampling information
   
        t=0;
        mm=0;
        for m=linspace(-1,1,NN)
            mm=mm+1;
            nn=0;
            for n=linspace(-1,1,NN)
                nn=nn+1;
                
                %generate slope data for Southwell method(slope data in a rectangular region) 
                corx(mm,nn)= n;
                cory(mm,nn)= m;        
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                
                if(m^2+n^2<=1)
                    t=t+1;
                    xmirror(t)=n;
                    ymirror(t)=m;
                end
            end
        end
        scancor=[xmirror',ymirror'];
        Ax= slope36x(scancor,NZern); % generate x slope matrix in circular region
        Ay= slope36y(scancor,NZern); % generate y slope matrix in circular region
        
        %%%%%%%%%%% data in rectangular region
        scancorRect=[corx(:),cory(:)];
        AxRect=slope36x(scancorRect,NZern);
        AyRect=slope36y(scancorRect,NZern);
    ZslopexRect= AxRect*coeff0/(hh1*1000/wavelength); % wavefront slope error in rectangular region
    ZslopeyRect= AyRect*coeff0/(hh1*1000/wavelength); % wavefront slope error in rectangular region
    spotshiftxRect=-ZslopexRect'*RR; % x spot shift in rectangular region
    spotshiftyRect=-ZslopeyRect'*RR; % y spot shift in rectangular region
    save slopeRect.mat  spotshiftxRect spotshiftyRect hh1 RR wavelength NN  % save data for Southwell zonal method

    
    xmirror0=xmirror*hh; % orignal spot positons
    ymirror0=ymirror*hh; %orignal spot positons
%     figure;plot(xmirror0,ymirror0,'+')
    Zslopex= Ax*coeff0/(hh1*1000/wavelength); % wavefront slope error in circular region
    Zslopey= Ay*coeff0/(hh1*1000/wavelength); % wavefront slope error in circular region
    spotshiftx=-Zslopex'*RR; % x spot shift in circular region
    spotshifty=-Zslopey'*RR;% y spot shift in circular region
    

%     figure;plot(spotshitx,spotshity,'+')


% add noise to spotd ata data
% spotshitx=spotshitx+randn(1,length(spotshitx))*0.001;
% spotshity=spotshity+randn(1,length(spotshitx))*0.001;



    Spotx=xmirror0+spotshiftx; % new spot postions
    Spoty=ymirror0+spotshifty; % new spot postions

    %plot surface

    if(plotrawdata==1)
        t=0;
        m=0;
        lsf0= coeff0;
        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



    figure(1);
    subplot(1,2,1)
    mesh(wfa);axis off;axis([0 101 0 101 -3 3]);

    subplot(1,2,2)
    plot(Spotx,Spoty,'.');axis equal;xlim([-hh,hh]);ylim([-hh,hh]);
%     hold
%     xx=[hh,-hh,-hh,hh,hh];
%     yy=[-hh,-hh,hh,hh,-hh];
%     plot(xx,yy,'-r')
    axis off
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc
disp('polunomial fitting')

Ax= slope36x(scancor,NZern);
Ay= slope36y(scancor,NZern);
A=[Ax;Ay];
Zslopes=-[spotshiftx';spotshifty']*(hh1*1000/wavelength)/RR; % slope data




CoeffAr=A\Zslopes % least square fit


ABX = Zslopes -(A*CoeffAr); % check fitting error
figure(2);plot(ABX);title('slope fitting residual');xlabel('sample points');ylabel('slope residuals')

%plot shape
if(plotmap==1)
    t=0;
    m=0;
    lsf0=CoeffAr;
    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);
    mesh(wfa);axis off;title('reconstructed map')
end
end