% (c) Copyright David Long, Brigham Young University, 2011.  All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This Matlab script illustrates how to process the BYU CASIE-09 
% sample data set using the range-Doppler algorithm (RDA) and 
% time-domain backprojection (BP) algorithm
%
% Written by Craig Stringham and David G Long, BYU 2011.
%
% Requires up to 4GB of RAM.  A separate file contains the
% prctile1 routine or the prctile routine from the matlab statistics
% toolbox can be used instead. Code is designed to illustrate the
% algorithm rather than for efficiency.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% close all, clear all;
PLOT=1;         % flag to enable plotting of images
RUNBP = 1;      % flag to enable computation of backprojection
updateTime=15;  % time between backprojection processing status updates
aresScale = 1;  % az resolution scale for BP. values>1 reduce resolution 
                % and computation time

%% code can optionally support matlab's use of GPU, set USEGPU to 0
% unless a GPU is available on your machine
USEGPU = 0;     % flag to enable use of GPU on capable platforms, zero
                % value disables use of GPU
matver = sscanf(version,'%d.%d.%d');
if matver(1) <= 7 && matver(2) < 12 && USEGPU == 1
    warning('This version of matlab does not support use of GPU')
    USEGPU = 0;
end

if USEGPU
    cgpuArray = @(x) single(gpuArray(x));
    cgather = @(x) gather(x);
else
    cgpuArray = @(x) x;
    cgather = @(x) x;
end

%% Set up microASAR SAR and processing parameters
fftsize = 4096*4;      % size of fft to use for backprojection range compression
N = ceil(fftsize/2);

c0 = 299702547;        % speed of light in the atmosphere (m/s)
actual_samples=1702;   % number of samples/chirp
PRF = 307.292;         % effective pulse repetition frequency (Hz)
azBW=0.1920;           % 3dB antenna azimuth beamwidth (rad) (3dB to 3dB)
cabledelay = -1.2;     % system and cable delay (m)
f_adc = 24485000;      % adc sample rate (Hz)
bw = 170000000;        % transmit bandwidth (Hz)
fc = 5.42876e9;        % carrier frequency (Hz)
kr = 1.597249139246997e12;   % chirp rate
delrad = c0*f_adc/(4*kr*pi); % meters per radian
delsamp = delrad*2*pi/actual_samples; % meters per real sample
delrsamp = delsamp*(actual_samples/fftsize); % meters per interpolated sample 
max_range = c0*f_adc/(4*kr); % maximum range without aliasing
lambda=c0/fc;          % carrier wave length
ares = lambda/(2*azBW)*aresScale; % azimuth resolution
rres = c0/(2*bw);      % slant range resolution

%% load the BYU CASIE-09 sample data
load('flight9_9_sample.mat')
numpulses = size(dat,2);
if 0 % optional code to reduce data set size for machines with <4GB memory
  Ndec = 8; % factor by which data set is reduced
  numpulses = floor(numpulses/Ndec);
  dat = dat(:,1:numpulses);
  geom = geom(:,1:numpulses);
end

%% range compress the data using a range window and zero padding
% Note that the first 30 samples are set to zero.  A Taylor window
% with NBar=4, SSL=-26 minimizes range sidelobes, but other windows
% can be used.
rc = fft(dat.*([zeros(30,1); taylorwin(actual_samples-30,4,-26)]*ones(1,numpulses)),fftsize,1);
% discard duplicated half of the spectrum 
rc = [rc(1:N,:); zeros(1,size(rc,2))];

% plot the range compressed data
if PLOT
    figure(1), imagesc(abs(rc),[0 5e4]), title('Magnitude range compressed image')
    ylabel('range index'); xlabel('azimuth index');
    drawnow
end


%% plot visuals of the flight geometry
t = (geom(1,:)-geom(1,1))/PRF; % time axis
if PLOT
    figure(2),
    th(1) = subplot(3,1,1);
    subplot(3,1,1), plot(t,geom(2,:))
    title('latitude')
    th(2) = subplot(3,1,2);
    subplot(3,1,2), plot(t,geom(3,:))
    title('longitude')
    th(3) = subplot(3,1,3);
    subplot(3,1,3), plot(t,geom(4,:))
    linkaxes(th,'x')
    title('altitude')
    drawnow
end

%% project lat lon onto a local tangent plane (northing easting surface)
RefLat=mean(geom(2,:));        % select a reference latitude
Conv=1852.23 * 60.0;           % latitude conversion factor
Lconv=Conv*cos(RefLat*pi/180); % longitude conversion factor
northing = geom(2,:)*Conv;
easting  = geom(3,:)*Lconv;
z = geom(4,:)-cabledelay ;     % correct height by cabledelay;

if PLOT
    figure(1),
    hold on, plot(z/delrsamp), hold off
end

%% rotate and move the gps data to a local reference
rotAngle = -atan2((easting(end)-easting(1)),(northing(end)-northing(1)))+pi/2;
cang = cos(rotAngle);
sang = sin(rotAngle);
l_northing = (northing-northing(1));
l_easting = easting-easting(1);
r = cang*l_northing-sang*l_easting;
a = sang*l_northing+cang*l_easting;
%%
if PLOT
    figure(3)
    plot(a(1),r(1),'gs',r(end),a(end),'rs',l_northing(1),l_easting(1),'gx',l_northing(end),l_easting(end),'rx'),
    figure(4)
    subplot(3,1,1), plot(a-a(end).*(t./t(end))), title('azimuth deviation'), ylabel('(m)')
    subplot(3,1,2), plot(r), title('ground range deviation'), ylabel('(m)')
    subplot(3,1,3), plot(z-mean(z)), title('altitude deviation'), ylabel('(m)'), xlabel('time index')
    drawnow
end


%% Backprojection Processing
if RUNBP
    disp('Beginning Backprojection processing')
    % set up grid for backprojection
    ground_range = sqrt(max_range.^2-mean(z).^2);
    y = cgpuArray(([ares:ares:a(end)]-ares/2).');
    x = cgpuArray([rres+30:rres:ground_range]-rres/2);
    z = cgpuArray(z);
    z2 = z.^2;
    % define image grid
    [X,Y] = meshgrid(x,y);
    % pre-compute constants in exponent
    betapic=4*pi*kr/c0.^2;
    lambda4pi=4*pi/lambda;
    % define output image and phase arrays
    img = cgpuArray(single(complex(zeros(size(X)),0)));
    pphase = cgpuArray(single(complex(zeros(size(X)),0)));
    
    % perform backprojection
    
    % use clock tick to measure CPU time
    inittic = tic;
    updr = inittic;
    if PLOT
        figure(10)
    end
        
    %% backprojection loop
    for k = 1:numpulses
        ty = Y-a(k);
        tx = X+r(k);
        tx2 = tx.^2;
        D2 = (ty.^2+tx2+z2(k));
        D = sqrt(D2); % distance from antenna to each pixel
        % get the azimuth angle estimate
        mz = sqrt(tx2+z(k).^2).*sign(tx); % can approximate by using mean(z) to lower computation
        az = atan2(ty,mz); % azimuth angle to each pixel
        ids = round(D./delrsamp);
        ids(ids>N) = N+1;
        pphase = exp(-1i*(betapic*D2-D*lambda4pi)); % calculate the expected phase
	% multiply and accumulate phase
        img = img+pphase.*reshape(rc(ids,k),size(ids,1),size(ids,2)).*(abs(az)<(azBW/2)).*exp(-az.^2*30);
        
        % plot an updated image
        tdr = toc(updr);
        if tdr > updateTime
            fprintf(1,'%d of %d complete, %fs elapsed, %fs estimated\n',k,numpulses,toc(inittic),toc(inittic)*(numpulses-k)/k)
            updr = tic;
            %
            if PLOT
                dimg = db(cgather(img));
                dimg(~isfinite(dimg)) = 0;
                rangescale = mean(dimg(1:find(cgather(y)-a(k)>0,1,'first'),:),1);
                rangescale = max(rangescale)./rangescale;
                rangescale(rangescale > 500) = 500;
                
                dimg = dimg.*(ones(length(y),1)*rangescale);
                imgMin = prctile1(dimg(dimg>0),10);
                imgMax = prctile1(dimg(dimg>0),99.99);
                set(0,'CurrentFigure',10); % this draws the update, but doesn't make the figure come to the foreground
                imagesc(x,y,dimg,[imgMin imgMax]), %colorbar
                set(gca,'YDir','normal'), colormap('gray')
                drawnow
                title('Single Look BP image')
            end
        end
      end
      
    %%
    if PLOT
        figure(10)
        imagesc(x,y,dimg,[imgMin imgMax]), colormap('gray')
        set(gca,'YDir','normal')
        title('Single Look BP image')
        xlabel('ground range (m)'); ylabel('azimuth (m)');
	drawnow
    end
    %%
    img = cgather(img); % bring the gpuArray back to the cpu memory if needed
    
    %% create multi-look backprojection image    
    NazLook=4;
    mimg=zeros([floor(size(img,1)/NazLook) size(img,2)]);
    for k=1:NazLook:size(img,1)-NazLook
        kk=(k-1)/NazLook+1;
        mimg(kk,:)=sum(abs(img(k:k+NazLook-1,:)).^2,1)/NazLook;
    end
    
    %% plot a multi look image
    
    if PLOT
        dmimg = db(cgather(mimg));
        dmimg(~isfinite(dmimg)) = 0;
        rangescale = mean(dmimg,1);
        rangescale = max(rangescale)./rangescale;
        rangescale(rangescale > 500) = 500;
        dmimg = dmimg.*(ones(size(dmimg,1),1)*rangescale);
        mimgMin = prctile1(dmimg(dmimg>0),10);
        mimgMax = prctile1(dmimg(dmimg>0),99.99);
        figure(11)
        imagesc(x,y,dmimg,[mimgMin mimgMax]), colormap('gray')
        set(gca,'YDir','normal')
        title('Multilook BP image')
        xlabel('ground range (m)'); ylabel('azimuth (m)');
	drawnow
    end
    disp('Backprojection processing completed')
end



%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% RDA Processing
disp('Beginning RDA processing')
rdafftsize = 4096;

%% RDA processing needs evenly spaced pulses data.  Since there are gaps
% we first need to fill in the missing pulses. What follows is a simple
% but effective interpolation method for the CASIE-09 SAR data.
pulsen = geom(1,:)-geom(1,1)+1;
dat1 = zeros(actual_samples,pulsen(end));
dat1(:,pulsen) = dat;
dpulsen=diff(pulsen)-1;
skips = pulsen(dpulsen~=0);
dpulsen = dpulsen(dpulsen~=0);
for k = 1:length(skips)
    dat1(:,skips(k)+1:skips(k)+dpulsen(k)) = dat1(:,skips(k)-dpulsen(k)+1:skips(k));
end
Naz = size(dat1,2);

%% interpolate the rest of the data
t1 =(1:pulsen(end))./PRF;
a1 = interp1(t,cgather(a),t1,'spline');
r1 = interp1(t,cgather(r),t1,'spline');
z1 = interp1(t,cgather(z),t1,'spline')+cabledelay;

%% range compress the data
% Note that the first 30 samples are set to zero.  A Taylor window
% with NBar=4, SSL=-226 minimizes range sidelobes, but other windows
% can be used.
rc1 = fft(dat1.*([zeros(30,1); taylorwin(actual_samples-30,4,-26)]*ones(1,Naz)),rdafftsize,1);
% discard duplicated half of FFT output
rc1 = rc1(1:rdafftsize/2,:);

% compute the slant range per sample
rdadelrsamp = delsamp*(actual_samples/rdafftsize);

%% find the mean linear velocity of the aircraft
vel = a1(end)./t1(end);

ran = (0:rdafftsize/2-1)*rdadelrsamp+cabledelay;
azm = ([1:Naz]-Naz/2-1)*vel/PRF;
% meshgrid az and range vars
[AZ RA] = meshgrid(azm,ran);

disp('calculating azimuth chirp')
% conventional parabolic azimuth chirp
% M = exp(-j*(fc*2/c0*(AZ.^2./(2*RA))-2*fc*vel*AZ./(c0.^2)));
% hyperbolic azimuth chirp
tau_r=2*sqrt(RA.^2+AZ.^2)/c0;
M=exp(-1i*(2*pi*fc*tau_r+kr*tau_r.^2));

% azimuth window (rect) based on antenna beamwidth
M(abs(AZ) > 0.5*RA*azBW) = 0.;
% simple tapered azimuth window
azwin=-600.0;
M=M.*exp(azwin*(AZ./RA).^2);

% fft shift to center frequency at origin
M=fftshift(M,2);

% convert azimuth chirp to frequency domain
M=fft(M,[],2);

% Take range compressed data to the range Doppler domain
A4=fft(rc1,[],2);

% Multiply by fft of azimuth chirp to do matched filtering
A4 = A4.*conj(M);

% Finish the process using ifft
A4 = ifft(A4,[],2);

% Apply an approximate range compensation for image display
rcomp=(ran').^3;
for k=1:size(A4,2)
    A4(:,k)=A4(:,k).*rcomp;
end


%% generate a georectified RDA image
grange = sqrt(([1:rres:sqrt((max_range-2)^2-mean(z1).^2)]).^2 + mean(z1).^2);
grange = grange(find(grange>mean(z1)+5,1,'first'):end);
gA4 = interp1([1:size(A4,1)]*rdadelrsamp,A4,grange);

% generate multilook image
NazLook=8;
C=zeros([size(A4,1) floor(size(A4,2)/NazLook)]);
gC=zeros([size(gA4,1) floor(size(gA4,2)/NazLook)]);
for k=1:NazLook:size(A4,2)-NazLook
    kk=(k-1)/NazLook+1;
    C(:,kk)=sum(abs(A4(:,k:k+NazLook-1)).^2,2)/NazLook;
    gC(:,kk)=sum(abs(gA4(:,k:k+NazLook-1)).^2,2)/NazLook;
end

imrange = ceil((mean(z1)+1)/rdadelrsamp):floor((max_range-2)/rdadelrsamp);


%% plot the RDA images
if PLOT 
    % display single look image
    figure(102)
    dA4 = db(rot90(A4(imrange,:)));
    dA4(find(dA4==-Inf))=NaN;
    r1low = prctile1(dA4(:),5);
    r1high = prctile1(dA4(:),99.9);
    azdis = [-vel*t(end)/2 vel*t(end)/2];
    imagesc(imrange*rdadelrsamp,azdis,dA4,[r1low r1high]);
    xlabel('slant range (m)'); ylabel('azimuth (m)');
    title(sprintf('Single look RDA image'))
    colormap('gray'), %colorbar
    
    % display multi look image
    figure(104)
    dC = db(rot90(C(imrange,:)));
    dC(find(dC==-Inf))=NaN;
    rmlow = prctile1(dC(:),3);
    rmhigh = prctile1(dC(:),99.99);
    imagesc(imrange*rdadelrsamp,azdis, dC,[rmlow rmhigh]);
    %colorbar
    xlabel('slant range (m)'); ylabel('azimuth (m)');
    colormap('gray')
    title(sprintf('Multilook RDA image'))    
    
    %% plot georectified multilook image
    figure(105)
    dgC = db(rot90(gC));
    dgC(find(dgC==-Inf))=NaN;
    rgmlow = prctile1(dgC(:),3);
    rgmhigh = prctile1(dgC(:),99.9);
    imagesc(sqrt(grange.^2-mean(z1)^2),azdis,dgC, [rgmlow rgmhigh]);
    %colorbar
    xlabel('ground range (m)'); ylabel('azimuth (m)');
    colormap('gray')
    title(sprintf('Georectified Multilook RDA image'))

end






