   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %   PULSED SPOTLIGHT SAR SIMULATION AND RECONSTRUCTION   %
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

colormap(gray(256))
cj=sqrt(-1);
pi2=2*pi;
%
c=3e8;                   % propagation speed
f0=50e6;                 % baseband bandwidth is 2*f0
w0=pi2*f0;
fc=200e6;                % carrier frequency
wc=pi2*fc;
lambda_min=c/(fc+f0);    % Wavelength at highest frequency
lambda_max=c/(fc-f0);    % Wavelength at lowest frequency
kc=(pi2*fc)/c;           % wavenumber at carrier frequency
kmin=(pi2*(fc-f0))/c;    % wavenumber at lowest frequency
kmax=(pi2*(fc+f0))/c;    % wavenumber at highest frequency
%
Xc=1000;                 % Range distance to center of target area
X0=20;                   % target area in range is within [Xc-X0,Xc+X0]
Yc=300;                  % Cross-range distance to center of target area
Y0=60;                  % target area in cross-range is within
                         % [Yc-Y0,Yc+Y0]

% Case 1: L < Y0; requires zero-padding of SAR signal in synthetic
% aperture domain
%
  L=100;                 % synthetic aperture is 2*L

% Case 2: L > Y0; slow-time Doppler subsampling of SAR signal spectrum
% reduces computation
%
% L=400;                 % synthetic aperture is 2*L

theta_c=atan(Yc/Xc);     % Squint angle
Rc=sqrt(Xc^2+Yc^2);      % Squint radial range
L_min=max(Y0,L);         % Zero-padded aperture is 2*L_min

%
Xcc=Xc/(cos(theta_c)^2); % redefine Xc by Xcc for squint processing

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% u domain parameters and arrays for compressed SAR signal %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
duc=(Xcc*lambda_min)/(4*Y0);      % sample spacing in aperture domain
                                  % for compressed SAR signal
duc=duc/1.2;                      % 10 percent guard band; this guard band
                                  % would not be sufficient for targets
                                  % outside digital spotlight filter (use
                                  % a larger guard band, i.e., PRF)
mc=2*ceil(L_min/duc);             % number of samples on aperture
uc=duc*(-mc/2:mc/2-1);            % synthetic aperture array
dkuc=pi2/(mc*duc);                % sample spacing in ku domain
kuc=dkuc*(-mc/2:mc/2-1);          % kuc array
%
dku=dkuc;                         % sample spacing in ku domain

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%    u domain parameters and arrays for SAR signal     %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
if Yc-Y0-L < 0,                            % minimum aspect angle
 theta_min=atan((Yc-Y0-L)/(Xc-X0));
else,
 theta_min=atan((Yc-Y0-L)/(Xc+X0));
end;
theta_max=atan((Yc+Y0+L)/(Xc-X0));         % maximum aspect angle
%
du=pi/(kmax*(sin(theta_max)- ...
                     sin(theta_min))); % sample spacing in aperture
                                       % domain for SAR signal
du=du/1.4;                        % 20 percent guard band
m=2*ceil(pi/(du*dku));            % number of samples on aperture
du=pi2/(m*dku);                   % readjust du
u=du*(-m/2:m/2-1);                % synthetic aperture array
ku=dku*(-m/2:m/2-1);              % ku array

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%       Fast-time domain parmeters and arrays          %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Tp=2.5e-7;                     % Chirp pulse duration
alpha=w0/Tp;                   % Chirp rate
wcm=wc-alpha*Tp;               % Modified chirp carrier
%
if Yc-Y0-L < 0,
 Rmin=Xc-X0;
else,
 Rmin=sqrt((Xc-X0)^2+(Yc-Y0-L)^2);
end;
Ts=(2/c)*Rmin;                 % start time of sampling
Rmax=sqrt((Xc+X0)^2+(Yc+Y0+L)^2);
Tf=(2/c)*Rmax+Tp;              % end time of sampling
T=Tf-Ts;                       % fast-time interval of measurement
Ts=Ts-.1*T;                    % start slightly earlier (10% guard band)
Tf=Tf+.1*T;                    % end slightly later (10% guard band)
T=Tf-Ts;
Tmin=max(T,(4*X0)/(c*cos(theta_max)));  % Minimum required T
%
dt=1/(4*f0);                 % Time domain sampling (guard band factor 2)
n=2*ceil((.5*Tmin)/dt);      % number of time samples
t=Ts+(0:n-1)*dt;             % time array for data acquisition
dw=pi2/(n*dt);               % Frequency domain sampling
w=wc+dw*(-n/2:n/2-1);        % Frequency array (centered at carrier)
k=w/c;                       % Wavenumber array
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Resolution for Broadside: (x,y) domain rotated by theta_c %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

DX=c/(4*f0);                      % range resolution (broadside)
DY=(Xcc*lambda_max)/(4*L);         % cross-range resolution (broadside)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           Parameters of Targets                 %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
ntarget=9;                        % number of targets
% Set ntarget=1 to see "clean" PSF of target at origin
% Try this with other targets

% xn: range;            yn= cross-range;    fn: reflectivity
  xn=zeros(1,ntarget);  yn=xn;              fn=xn;

% Targets within digital spotlight filter
%
  xn(1)=0;              yn(1)=0;            fn(1)=1;
  xn(2)=.7*X0;          yn(2)=-.6*Y0;       fn(2)=1.4;
  xn(3)=0;              yn(3)=-.85*Y0;      fn(3)=.8;
  xn(4)=-.5*X0;         yn(4)=.75*Y0;       fn(4)=1.;
  xn(5)=-.5*X0+DX;      yn(5)=.75*Y0+DY;    fn(5)=1.;

% Targets outside digital spotlight filter
% (Run the code with and without these targets)
%
  xn(6)=-1.2*X0;        yn(6)=.75*Y0;       fn(6)=1.;
  xn(7)=.5*X0;          yn(7)=1.25*Y0;      fn(7)=1.;
  xn(8)=1.1*X0;         yn(8)=-1.1*Y0;      fn(8)=1.;
  xn(9)=-1.2*X0;        yn(9)=-1.75*Y0;     fn(9)=1.;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                   SIMULATION                    %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
s=zeros(n,mc);     % SAR signal array
%
for i=1:ntarget;   % Loop for each target
 td=t(:)*ones(1,mc)-2*ones(n,1)*sqrt((Xc+xn(i)).^2+(Yc+yn(i)-uc).^2)/c;
 s=s+fn(i)*exp(cj*wcm*td+cj*alpha*(td.^2)).*(td >= 0 & td <= Tp & ...
   ones(n,1)*abs(uc) <= L & t(:)*ones(1,mc) < Tf);
end;
%
s=s.*exp(-cj*wc*t(:)*ones(1,mc));      % Fast-time baseband conversion

% User may apply a slow-time domain window, e.g., power window, on
% simulated SAR signal array "s" here.

G=abs(s)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(t,uc,256-cg*(G-ng));
axis('square');axis('xy')
xlabel('Fast-time t, sec')
ylabel('Synthetic Aperture (Slow-time) U, meters')
title('Measured Spotlight SAR Signal')
print P5.1.ps
pause(1)
%

td0=t(:)-2*sqrt(Xc^2+Yc^2)/c;
s0=exp(cj*wcm*td0+cj*alpha*(td0.^2)).*(td0 >= 0 & td0 <= Tp);
s0=s0.*exp(-cj*wc*t(:));            % Baseband reference fast-time signal

s=ftx(s).*(conj(ftx(s0))*ones(1,mc));  % Fast-time matched filtering
%
G=abs(iftx(s))';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
tm=(2*Rc/c)+dt*(-n/2:n/2-1);    % fast-time array after matched filtering
image(tm,uc,256-cg*(G-ng));
axis('square');axis('xy')
xlabel('Fast-time t, sec')
ylabel('Synthetic Aperture (Slow-time) U, meters')
title('SAR Signal after Fast-time Matched Filtering')
print P5.2.ps
pause(1)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Slow-time baseband conversion for squint %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
kus=2*kc*sin(theta_c)*ones(1,n);     % Doppler frequency shift in ku
                                     % domain due to squint
%
s=s.*exp(-cj*kus(:)*uc);             % slow-time baseband conversion
fs=fty(s);

% Display aliased SAR spectrum
%
G=abs(fs)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(k*c/pi2,kuc,256-cg*(G-ng));
axis('square');axis('xy')
xlabel('Fast-time Frequency, Hertz')
ylabel('Synthetic Aperture (Slow-time) Frequency Ku, rad/m')
title('Aliased Spotlight SAR Signal Spectrum')
print P5.3.ps
pause(1)

%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Digital Spotlighting and Bandwidth Expansion in ku Domain  %%
%%          via Slow-time Compression and Decompression        %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
s=s.*exp(cj*kus(:)*uc);      % Original signal before baseband
                             % conversion for squint

cs=s.*exp(cj*2*(k(:)*ones(1,mc)).* ...
 (ones(n,1)*sqrt(Xc^2+(Yc-uc).^2))-cj*2*k(:)*Rc*ones(1,mc));% compression
fcs=fty(cs);            % F.T. of compressed signal w.r.t. u
%
G=abs(fcs)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(k*c/pi2,kuc,256-cg*(G-ng));
axis('square');axis('xy')
xlabel('Fast-time Frequency, Hertz')
ylabel('Synthetic Aperture (Slow-time) Frequency Ku, rad/m')
title('Compressed Spotlight SAR Signal Spectrum')
print P5.4.ps
pause(1)
%
fp=iftx(fty(cs));      % Narrow-bandwidth Polar Format Processed
                       % reconstruction
%
PH=asin(kuc/(2*kc));   % angular Doppler domain
R=(c*tm)/2;            % range domain mapped from reference
                       % fast-time domain
%
% Full Aperture Digital-Spotlight Filter
%
W_d=((abs(R(:)*cos(PH+theta_c)-Xc) < X0).* ...
    (abs(R(:)*sin(PH+theta_c)-Yc) < Y0));
%
G=(abs(fp)/max(max(abs(fp)))+.1*W_d)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image((Rc/Xc)*(.5*c*tm-Rc),(kuc*Rc)/(2*kc),256-cg*(G-ng));
xlabel('Range x, m')
ylabel('Cross-range y, m')
title('Polar Format SAR Reconstruction with Digital Spotlight Filter')
axis image; axis xy;
print P5.5.ps
pause(1)

fd=fp.*W_d;                % Digital Spotlight Filtering
fcs=ftx(fd);               % Transform to (omega,ku) domain

% Zero-padding in ku domain for slow-time upsampling
%
mz=m-mc;        % number is zeros
fcs=(m/mc)*[zeros(n,mz/2),fcs,zeros(n,mz/2)];
%
cs=ifty(fcs);              % Transform to (omega,u) domain

s=cs.*exp(-cj*2*(k(:)*ones(1,m)).* ...
 (ones(n,1)*sqrt(Xc^2+(Yc-u).^2))+cj*2*k(:)*Rc*ones(1,m));% decompression

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           CAUTION                             %
% For TDC or backprojection, do not subsample in Doppler domain %
% and do not perform slow-time baseband conversion               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
s_ds=s;                    % Save s(omega,u) array for TDC and
                           % backprojection algorithms

%
s=s.*exp(-cj*kus(:)*u);    % Slow-time baseband conversion for squint
fs=fty(s);                 % Digitally-spotlighted SAR signal spectrum
%
G=abs(fs)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(k*c/pi2,ku,256-cg*(G-ng));
axis('square');axis('xy')
xlabel('Fast-time Frequency, Hertz')
ylabel('Synthetic Aperture (Slow-time) Frequency Ku, rad/m')
title('Spotlight SAR Signal Spectrum after DS & Upsampling')
print P5.6.ps
pause(1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%    SLOW-TIME DOPPLER SUBSAMPLING     %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
if Y0 < L,
 ny=2*ceil(1.2*Y0/du);      % Number of samples in y domain
                            % 20 percent guard band
 ms=floor(m/ny);            % subsampling ratio
 tt=floor(m/(2*ms));
 I=m/2+1-tt*ms:ms:m/2+1+(tt-1)*ms; % subsampled index in ku domain
 [tt,ny]=size(I);           % number of subsamples
 fs=fs(:,I);                % subsampled SAR signal spectrum
 ky=ku(I);                  % subsampled ky array
 dky=dku*ms;                % ky domain sample spacing
else,
 dky=dku;
 ny=m;
 ky=ku;
end;

dy=pi2/(ny*dky);            % y domain sample spacing
y=dy*(-ny/2:ny/2-1);        % cross-range array

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%             RECONSTRUCTION           %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
ky=ones(n,1)*ky+kus(:)*ones(1,ny);       % ky array
kx=(4*k(:).^2)*ones(1,ny)-ky.^2;
kx=sqrt(kx.*(kx > 0));                  % kx array
%
plot(kx(1:20:n*ny),ky(1:20:n*ny),'.')
xlabel('Spatial Frequency k_x, rad/m')
ylabel('Spatial Frequency k_y, rad/m')
title('Spotlight SAR Spatial Frequency Data Coverage')
axis image; axis xy
print P5.7.ps
pause(1)
%
kxmin=min(min(kx));
kxmax=max(max(kx));
dkx=pi/X0;        % Nyquist sample spacing in kx domain
nx=2*ceil((.5*(kxmax-kxmin))/dkx); % Required number of
                      % samples in kx domain;
                      % This value will be increased slightly
                      % to avoid negative array index
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                                         %%%
%%%   FIRST TWO OPTIONS FOR RECONSTRUCTION:                 %%%
%%%                                                         %%%
%%%     1. 2D Fourier Matched Filtering and Interpolation   %%%
%%%     2. Range Stacking                                   %%%
%%%                                                         %%%
%%%     Note: For "Range Stacking," make sure that the      %%%
%%%           arrays nx, x, and kx are defined.             %%%
%%%                                                         %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%    2D FOURIER MATCHED FILTERING AND INTERPOLATION    %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Matched Filtering
%
fs0=(kx > 0).*exp(cj*kx*Xc+cj*ky*Yc+cj*.25*pi ...
           -cj*2*k(:)*ones(1,ny)*Rc); % reference signal complex conjugate
fsm=fs.*fs0;     % 2D Matched filtering

% Interpolation
%
is=8;       % number of neighbors (sidelobes) used for sinc interpolator
I=2*is+1;
kxs=is*dkx; % plus/minus size of interpolation neighborhood in KX domain
%
nx=nx+2*is+4;  % increase number of samples to avoid negative
               %  array index during interpolation in kx domain
KX=kxmin+(-is-2:nx-is-3)*dkx;     % uniformly-spaced kx points where
                                  % interpolation is done
kxc=KX(nx/2+1);                   % carrier frequency in kx domain
KX=KX(:)*ones(1,ny);
%
F=zeros(nx,ny);         % initialize F(kx,ky) array for interpolation

for i=1:n;                       % for each k loop
  i                              % print i to show that it is running
 icKX=round((kx(i,:)-KX(1,1))/dkx)+1; % closest grid point in KX domain
 cKX=KX(1,1)+(icKX-1)*dkx;            % and its KX value
 ikx=ones(I,1)*icKX+[-is:is]'*ones(1,ny);
 ikx=ikx+nx*ones(I,1)*[0:ny-1];
 nKX=KX(ikx);
 SINC=sinc((nKX-ones(I,1)*kx(i,:))/dkx);             % interpolating sinc
 HAM=.54+.46*cos((pi/kxs)*(nKX-ones(I,1)*kx(i,:)));  % Hamming window
         %%%%%   Sinc Convolution (interpolation) follows  %%%%%%%%
 F(ikx)=F(ikx)+(ones(I,1)*fsm(i,:)).*(SINC.*HAM);
end
%
%  DISPLAY interpolated spatial frequency domain image F(kx,ky)

KX=KX(:,1).';
KY=ky(1,:);

G=abs(F)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(KX,KY+kus(1),256-cg*(G-ng));
axis image; axis xy
xlabel('Spatial Frequency k_x, rad/m')
ylabel('Spatial Frequency k_y, rad/m')
title('Wavefront Spotlight SAR Reconstruction Spectrum')
print P5.8.ps
pause(1)

%
f=iftx(ifty(F));     % Inverse 2D FFT for spatial domain image f(x,y)
%
dx=pi2/(nx*dkx);     % range sample spacing in reconstructed image
x=dx*(-nx/2:nx/2-1); % range array
%
% Display SAR reconstructed image

G=abs(f)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(Xc+x,Yc+y,256-cg*(G-ng));axis([Xc-X0 Xc+X0 Yc-Y0 Yc+Y0]);
axis image; axis xy
xlabel('Range X, meters')
ylabel('Cross-range Y, meters')
title('Wavefront Spotlight SAR Reconstruction')
print P5.9.ps
pause(1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   SAR Image Compression (for Spotlight System)  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
Fc=ftx(fty(f.* ...
  exp(cj*kxc*x(:)*ones(1,ny)+cj*ones(nx,1)*2*kc*sin(theta_c)*y ...
 -cj*2*kc*sqrt(((Xc+x(:)).^2)*ones(1,ny)+ones(nx,1)*((Yc+y).^2)))));
G=abs(Fc)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(KX,KY+kus(1),256-cg*(G-ng));
axis image; axis xy
xlabel('Spatial Frequency k_x, rad/m')
ylabel('Spatial Frequency k_y, rad/m')
title('Compressed Spotlight SAR Reconstruction Spectrum')
print P5.10.ps
pause(1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%      RANGE STACK WAVEFRONT RECONSTRUCTION       %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
f_stack=zeros(nx,ny); % Initialize reconstruction array in (x,y) domain
for i=1:nx; i        % Stack's loop for reconstruction at each range
 f_stack(i,:)=ifty(sum(fs.*exp(cj*kx*(Xc+x(i))+cj*ky*Yc ...
  +cj*.25*pi-cj*2*k(:)*ones(1,ny)*Rc)));
end;

% Remove carrier in range domain
f_stack=f_stack.*exp(-cj*x(:)*kxc*ones(1,ny));
%
f_stack=f_stack/nx; % Scale it for comparison with Fourier interpolation
                    % Use "f_stack-f" to display difference of two
                    % reconstructions

G=abs(f_stack)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(Xc+x,Yc+y,256-cg*(G-ng));axis([Xc-X0 Xc+X0 Yc-Y0 Yc+Y0]);
axis image; axis xy
xlabel('Range X, meters')
ylabel('Cross-range Y, meters')
title('Range Stack Spotlight SAR Reconstruction')
print P5.11.ps
pause(1)

F_stack=ftx(fty(f_stack)); % Reconstruction array in spatial frequency
                           % domain; Use "F_stack-F" to display
                           %  difference of two reconstructions
%
G=abs(F_stack)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(KX,KY+kus(1),256-cg*(G-ng));
axis image; axis xy
xlabel('Spatial Frequency k_x, rad/m')
ylabel('Spatial Frequency k_y, rad/m')
title('Range Stack Spotlight SAR Reconstruction Spectrum')
print P5.12.ps
pause(1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%     TIME DOMAIN CORRELATION RECONSTRUCTION      %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
f_tdc=zeros(nx,ny); % Initialize reconstruction array in (x,y) domain

for i=1:nx; i
   for j=1:ny;
      t_ij=(2*sqrt((x(i)+Xc)^2+(y(j)+Yc-u).^2))/c;
      f_tdc(i,j)=sum(sum(s_ds.*exp(cj*w(:)*(t_ij-tm(n/2+1))).* ...
         (ones(n,1)*(t_ij >= Ts & t_ij <= Tf))));
   end;
end;

% Remove carrier in range domain
f_tdc=f_tdc.*exp(-cj*x(:)*kxc*ones(1,ny));
%

% Remove carrier in cross-range domain (squint mode)
f_tdc=f_tdc.*exp(-cj*ones(nx,1)*2*kc*sin(theta_c)*y);

G=abs(f_tdc)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(Xc+x,Yc+y,256-cg*(G-ng));axis([Xc-X0 Xc+X0 Yc-Y0 Yc+Y0]);
axis image; axis xy
xlabel('Range X, meters')
ylabel('Cross-range Y, meters')
title('TDC Spotlight SAR Reconstruction')
print P5.13.ps
pause(1)

F_tdc=ftx(fty(f_tdc));     % Reconstruction array in spatial frequency
%
G=abs(F_tdc)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(KX,KY+kus(1),256-cg*(G-ng));
axis image; axis xy
xlabel('Spatial Frequency k_x, rad/m')
ylabel('Spatial Frequency k_y, rad/m')
title('TDC Spotlight SAR Reconstruction Spectrum')
print P5.14.ps
pause(1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%           BACKPROJECTION RECONSTRUCTION         %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
f_back=zeros(nx,ny); % Initialize reconstruction array in (x,y) domain
n_ratio=100;              % Upsampling ratio in fast-time domain
nu=n_ratio*n;             % Size of upsampled s(t,u) array in t domain
nz=nu-n;                  % Number of zeros
dtu=(n/nu)*dt;            % Fast-time sample spacing of upsampled array
tu=dtu*(-nu/2:nu/2-1);    % Upsampled reference fast-time array
X=x(:)*ones(1,ny);
Y=ones(nx,1)*y;

for j=1:m; j
   t_ij=(2*sqrt((X+Xc).^2+(Y+Yc-u(j)).^2))/c;
   t_ij=round((t_ij-tm(n/2+1))/dtu)+nu/2+1;
   it_ij=(t_ij > 0 & t_ij <= nu);
   t_ij=t_ij.*it_ij+nu*(1-it_ij);
   S=ifty([zeros(1,nz/2),s_ds(:,j).',zeros(1,nz/2)])...
      .*exp(cj*wc*tu);
   S(nu)=0;
   f_back=f_back+S(t_ij);
end;

clear X Y

% Remove carrier in range domain
f_back=f_back.*exp(-cj*x(:)*kxc*ones(1,ny));
%

% Remove carrier in cross-range domain (squint mode)
f_back=f_back.*exp(-cj*ones(nx,1)*2*kc*sin(theta_c)*y);

G=abs(f_back)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(Xc+x,Yc+y,256-cg*(G-ng));axis([Xc-X0 Xc+X0 Yc-Y0 Yc+Y0]);
axis image; axis xy
xlabel('Range X, meters')
ylabel('Cross-range Y, meters')
title('Backprojection Spotlight SAR Reconstruction')
print P5.15.ps
pause(1)

F_back=ftx(fty(f_back));     % Reconstruction array in spatial frequency
%
G=abs(F_back)';
xg=max(max(G)); ng=min(min(G)); cg=255/(xg-ng);
image(KX,KY+kus(1),256-cg*(G-ng));
axis image; axis xy
xlabel('Spatial Frequency k_x, rad/m')
ylabel('Spatial Frequency k_y, rad/m')
title('Backprojection Spotlight SAR Reconstruction Spectrum')
print P5.16.ps
pause(1)


function [L_t, S_t, iters, frob_err] = ncrpca(M, true_r, EPS, MAX_ITER, EPS_S, incoh, TOL)

% This matlab code implements Non-convex Robust PCA (NcRPCA)
% Input:
% M = given low rank+sparse matrix to be decomposed
% true_r = maximum rank of the low rank rank component
% EPS (optional) = convergence threshold for ||M-(L_t+S_t)||_F; default is 1e-3
% MAX_ITER (optional) = maximum iterations for NcRPCA; default is 51
% EPS_S (optional) = threshold for removing small entries in the sparse component; default is 1e-3
% incoh (optional) = incoherence of the low rank component; default is 1
% TOL (optional) = tolerance for relative error in ||M-(L_t+S_t)||_F in consecutive iterations; default is 1e-1
% Output:
% M_t = thresholded M at each iteration
% L_t = rank-k approximation of M_t
% S_t = sparse component, computed as M-M_t
% iters = number of iteration of NcRPCA
% frob_err = ||M-(L_t+S_t)||_F at each iteration

if nargin < 7, TOL = 1e-1; end
if nargin < 6, incoh = 1; end
if nargin < 5, EPS_S = 1e-3; end
if nargin < 4, MAX_ITER = 51; end
if nargin < 3, EPS = 1e-3; end

%addpath code_ncrpca/PROPACK;
%addpath PROPACK;
frob_err(1) = inf;
[~, n] = size(M);
t = 1;
idx = [];
thresh_const = incoh; % threshold constant: can be tuned depending on incoherence
thresh_red = 0.9; % parameter to reduce the threshold constant: can be tuned
r_hat = 1; % initial rank for stagewise algorithm
L_t = zeros(size(M));
Sig_t = lansvd(M,1,'L');
D_t = M-L_t;
thresh = thresh_const*Sig_t/sqrt(n);
idx = unique([find(abs(D_t) > thresh); idx]);
S_t = zeros(size(M));
S_t(idx) = D_t(idx); % initial thresholding
if max(idx(:))==0
    idx = [];
end
while frob_err(t)/norm(M, 'fro')>=EPS && t<MAX_ITER % convergence check
    if ~mod(t, 10)
        %fprintf('Iter no. %d\n', t);
    end
    t = t+1;
    [U_t, Sig_t, V_t] = lansvd(M-S_t, r_hat+1, 'L');
    %[U_t, Sig_t, V_t] = svds(M-S_t, r_hat+1); % use this if not using propack
    L_t= U_t(:,1:r_hat)*Sig_t(1:r_hat,1:r_hat)*V_t(:,1:r_hat)';
    D_t = M-L_t;
    thresh = (thresh_const/sqrt(n))*Sig_t(r_hat+1, r_hat+1); % use n instead of sqrt(n) for thresholding less aggresively
    idx = unique([find(abs(D_t) > thresh); idx]);
    S_t(idx) = D_t(idx);
    frob_err(t) = norm(M-(L_t+S_t), 'fro');
    if ((frob_err(t-1)-frob_err(t))/frob_err(t-1) <= TOL) && r_hat<true_r
%         r_hat = r_hat+1; % use this for incrementally updating rank by 1
        sig_t = lansvd(M-S_t, true_r, 'L'); % svd function from propack
        ratio_sig = sig_t(r_hat+1:end)./[sig_t(r_hat+2:end); sig_t(end)];
        [~, mx_idx] = max(ratio_sig);
        r_hat = r_hat+mx_idx; % update rank for the next stage
    elseif ((frob_err(t-1)-frob_err(t))/frob_err(t-1) <= TOL) && r_hat==true_r
        thresh_const = thresh_const*thresh_red; % tune threshold
    end
end
S_t(abs(S_t)<EPS_S) = 0; % threshold to remove small errors and obtain sparse component
iters = length(frob_err)-1; % no. of iters. of ncrpca = length(frob_err)-1
end


function hand_translation ( )

%*****************************************************************************80
%
%% HAND_TRANSLATION applies a translation xy2 = xy + b to hand data.
%
%  Discussion:
%
%     This program assumes that the file 'HAND_NODES.TXT' is available.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Cleve Moler,
%    Numerical Computing with MATLAB,
%    SIAM, 2004,
%    ISBN13: 978-0-898716-60-3,
%    LC: QA297.M625.
%

%
%  Read the data.
%
  xy = load ( 'hand_nodes.txt' );
%
%  Make XY an array of column vectors.
%
  xy = xy';
%
%  Repeat the first column at the end so the polygon closes.
%
  xy = [ xy, [ xy(:,1) ] ];
%
%  Define the transformation as xy2 = xy + b.
%
  b = repmat ( [ 1.0; -0.5 ], 1, 60 );
%
%  Transform the data.
%
  xy2 = xy + b;
%
%  Clear the graphics frame.
%
  clf
%
%  Plot the original data.
%
  plot ( xy(1,:), xy(2,:), 'Color', 'r', 'LineWidth', 2 );
%
%  Plot the transformed data.
%
  hold on
  plot ( xy2(1,:), xy2(2,:), 'Color', 'b', 'LineWidth', 2 );
%
%  Annotate.
%
  axis equal
  grid on
  title ( 'Data (red), and data+[1.0;-0.5] (blue)' )

  hold off

  return
end


 function G = Gtomo2_strip(sg, ig, varargin)
%function G = Gtomo2_strip(sg, ig, options)
%
% Generate a 2D system matrix for tomographic image reconstruction,
% based on a square pixel basis and strip-integral detector model,
% i.e., a rectangular detector response.
% Works for parallel-beam, arc-fan, and flat-fan geometries.
%
% Closely matches Aspire "system 13" and "system 14".
% The main limitation here is that it uses Matlab "sparse" data type,
% so you will run out of memory for large image or sinogram sizes.
%
% in:
%	sg		sino_geom()
%	ig		image_geom()
%
% options:
%	'strip_width'	if 0, then line integrals (default: sg.d)
%	'single'	if 1, then double(single()) the values, cf aspire
%	'gam_max'	max acceptance angle of detector [degrees] (def: 90)
%
% out:
%	G [nb*na,np]	Gsparse object, where np = sum(ig.mask(:))
%
% Copyright 2005-8-16, Jeff Fessler, The University of Michigan

if nargin == 1 && streq(sg, 'test'), Gtomo2_strip_test, return, end
if nargin < 2, help(mfilename), error(mfilename), end

% defaults
arg.gam_max = 90; % degrees
arg.strip_width = [];
arg.single = false;
arg.chat = false;
arg.sse = false;
arg = vararg_pair(arg, varargin);

if isempty(arg.strip_width), arg.strip_width = sg.d; end

if streq(sg.type, 'par')
  if (arg.sse)

    nthread = 1;
    mask = uint8(ig.mask);

    [A B] = jmh_sse_mex('jmh,sse,matrix',int32(nthread), ...
                        single(sg.orbit_start*pi/180), single(sg.orbit*pi/180), ...
                        int32(sg.nb), single(sg.dr), single(sg.offset_r), ...
                        single(arg.strip_width), int32(ig.nx), single(ig.dx), ...
                        single(ig.offset_x), int32(ig.ny), single(ig.dy), ...
                        single(ig.offset_y), mask,int32(sg.na));

    M = ceil((ig.dx * sqrt(2) + arg.strip_width) / sg.dr);

%     jj = [];

%     % Bad idea
%     for y=1:ig.ny
%       for x=1:ig.nx
%         for a=1:sg.na
%           for m=1:M
%             jj = [jj,(y-1)*ig.nx + x];
%           end
%         end
%       end
%     end
    jj = reshape(repmat(1:ig.nx*ig.ny,sg.na*M,1),[1,ig.nx*ig.ny*sg.na*M]);

    G = sparse(double(B+1),double(jj'),double(A),sg.na*sg.nb,ig.nx*ig.ny,length(A));

  else
      G = Gtomo2_strip_par(sg.nb, sg.na, sg.dr, sg.offset_r, ...
                           sg.orbit, sg.orbit_start, arg.strip_width, ...
                           ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
                           ig.mask, arg.single);
  end
elseif streq(sg.type, 'fan')
	G = Gtomo2_strip_fan(sg.nb, sg.na, sg.ds, sg.offset_s, ...
		sg.ar, arg.strip_width, ...
		deg2rad(arg.gam_max), ...
		sg.dsd, sg.dso, sg.dfs, ...
		sg.source_offset, ...
		ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
		ig.mask, arg.single, arg.chat);

else
	error('unknown geometry: %s', sg.type)
end

G = Gsparse(G, 'mask', ig.mask, 'idim', [ig.nx ig.ny], 'odim', [sg.nb sg.na]);

%
% = Gtomo2_strip_par()
%
function G = Gtomo2_strip_par(nb, na, dr, offset_r, ...
	orbit, orbit_start, strip_width, ...
	nx, ny, dx, dy, offset_x, offset_y, mask, is_single);

% pixel centers
wx = (nx-1)/2 + offset_x;
wy = (ny-1)/2 + offset_y;
wb = (nb-1)/2 + offset_r;

x = dx * ([0:nx-1] - wx);
y = dy * ([0:ny-1] - wy); % caution, may not match aspire if offset_y != 0
[x y] = ndgrid(x, y);
x = x(mask(:));
y = y(mask(:));
np = length(x);		% sum(mask(:)) - total # of support pixels

angle = deg2rad(orbit_start + [0:na-1]'/na * orbit);
cang = cos(angle);
sang = sin(angle);
tau = cang * x' + sang * y'; % [na,np] projected pixel center
tau = tau / dr;
tau = tau + wb;

d_max = (abs(cang) + abs(sang)) / 2;
tau_max = (dx * d_max + strip_width/2) / dr;
ib_min = 1 + floor(tau - repmat(tau_max, 1, np));
M = ceil((dx * sqrt(2) + strip_width) / dr);
jj = find(mask(:))'; % all-column G
jj = repmat(jj, na, 1);
jj = col(jj); % so that na=1 case works
list.ii = [];
list.jj = [];
list.ss = [];
for mm=0:M-1
	ticker(mfilename, mm+1, M)
	ib = ib_min + mm;
	good = (ib >= 0) & (ib < nb);
	good = good(:);
	val = square_strip_int(dr * (ib - tau), ...
		repmat(angle, 1, np), 'dx', dx, 'sw', strip_width);
	val = val(:);
	ii = col(ib + repmat([0:na-1]'*nb, 1, np)); % sinogram index
	list.ii = [list.ii; 1+ii(good)];
	list.jj = [list.jj; jj(good)];
	list.ss = [list.ss; val(good)];
end
if is_single
	list.ss = double(single(list.ss)); % stupid matlab insists on double
end
G = sparse(list.ii, list.jj, list.ss, nb*na, nx*ny, length(list.ss));

%
% = Gtomo2_strip_fan()
%
function G = Gtomo2_strip_fan(nb, na, ds, offset_s, ...
	beta, strip_width, ...
	gam_max, ... % maximum angle gamma accepted by detector [radians]
	dsd, dso, dfs, roff, ...
	nx, ny, dx, dy, offset_x, offset_y, mask, is_single, chat);

% pixel centers
wx = (nx-1)/2 + offset_x;
wy = (ny-1)/2 + offset_y;
wb = (nb-1)/2 + offset_s;

x = dx * ([0:nx-1] - wx);
y = dy * ([0:ny-1] - wy); % caution, may not match aspire if offset_y != 0
[x y] = ndgrid(x, y);
x = x(mask(:)); % [np,1]
y = y(mask(:));
np = length(x); % sum(mask(:)) - total # of support pixels
na = length(beta);

cbet = cos(beta);
sbet = sin(beta);

% s0 is "s" value for center of each pixel
gam0 = atan2(cbet * x' + sbet * y' - roff, ...
	dso + sbet * x' - cbet * y'); % [na,np]
clear cbet sbet
if dfs == 0 % 3rd gen (arc)
	s0 = dsd * gam0;
elseif isinf(dfs)
	s0 = dsd * tan(gam0);
else
	error 'unsupported dfs'
end

ang0 = repmat(beta, [1 np]) + gam0;
mag0 = dso * cos(gam0) - roff * sin(gam0) ...
	+ repmat(x', [na 1]) .* sin(ang0) - repmat(y', [na 1]) .* cos(ang0);
gamgood = abs(gam0) < gam_max;
if chat, printm('gam0: %g %g', rad2deg(minmax(gam0(gamgood)))), end
clear gam0
if dfs == 0 % 3rd gen (arc)
	mag0 = mag0 / dsd;
elseif isinf(dfs)
	mag0 = mag0 / dsd ./ (1 + (s0 ./ dsd).^2);
else
	error 'dfs bug'
end
mag0min = min(mag0(gamgood));
if isempty(mag0min)
	G = sparse([], [], [], nb*na, nx*ny, 0);
	return
end

tau = s0 / ds + wb; % [na,np], unitless

M = ceil((dx/mag0min * sqrt(2) + strip_width) / ds); % conservative!
if chat, printm('M=%d, mag0min=%g', M, mag0min), end
if M > 100
	printm('Warn: M=%d too large? probably x-ray source is too oblique', M)
	printm('type "return" and hope for the best, but it may be *slow*')
	printm('recommend using "gam_max" to impose incidence angle constraint')
	keyboard
end

ib_min = 1 + floor(tau - M/2);
jj = find(mask(:))';	% all-column G
jj = repmat(jj, na, 1); % [na,np]
jj = col(jj); % so that na=1 case works
list.ii = [];
list.jj = [];
list.ss = [];
for mm=0:M-1
	ticker(mfilename, mm+1, M)
	ib = ib_min + mm;
	val = mag0 .* square_strip_int(ds * (ib - tau), ...
		ang0, 'dx', dx ./ mag0, ...
		'sw', strip_width);
	val = val(:); % for na=1 case
%	val = square_strip_int(ds * (ib - tau) .* mag0, ...
%		ang0, 'dx', dx, 'sw', strip_width .* mag0); % same!
	ii = col(ib + repmat([0:na-1]'*nb, 1, np)); % sinogram index
%	if chat, printm('%d of %d zeros', sum(val(:)==0), length(val(:))), end
	good = (ib >= 0) & (ib < nb);
	good = good & gamgood;
	good = good(:);
	good = good & (val > 0); % remove extra zeros due to conservative M
	list.ii = [list.ii; 1+ii(good)];
	list.jj = [list.jj; jj(good)];
	list.ss = [list.ss; val(good)];
end
if is_single
	list.ss = double(single(list.ss)); % stupid matlab insists on double
end
G = sparse(list.ii, list.jj, list.ss, nb*na, nx*ny, length(list.ss));

%
% test demo
%
function Gtomo2_strip_test
ig = image_geom('nx', 512, 'ny', 480', 'fov', 500, 'down', 8);
ig.mask = ig.circ > 0;
% todo: 3 cases to test: parallel, arc-fan, flat-fan
sg = sino_geom('par', 'nb', 600, 'na', 480, 'dr', 1.2, 'down', ig.down);
% sg = sino_geom('fan', 'nb', 888, 'na', 984, 'ds', 1.0, 'down', ig.down, ...
% 	'offset_s', 0*0.25, 'source_offset', 0*3.0, ...
% 	'dsd', 949, 'dod', 408, 'dfs', 0);

ell = [20 50 150 150 0 1];
ell = [];
[x ell] = ellipse_im(ig, ell, 'oversample', 2);
ya = ellipse_sino(sg, ell, 'oversample', 4);

G = Gtomo2_strip(sg, ig, 'chat', 0);

yd = G * x;
sino = sg.zeros; sino(sg.nb/2, 10) = 1;
b1 = G' * sino;
bu = G' * sg.ones;
max_percent_diff(min(bu(ig.mask)), max(bu(ig.mask)), 'backproject ones')

if im
	clf, im pl 2 2
	im(1, x, 'test image')
	im(2, ya, 'sinogram ya'), cbar
	im(4, yd, 'sinogram yd'), cbar
	im(3, yd-ya, 'yd-ya'), cbar
	if 0
		im(1, ig.mask, 'support mask')
		im(2, b1, 'backproject 1 ray'), cbar
		im(3, bu, 'backproject ones'), cbar
	end
end

% verify consistency with Gtomo2_wtmex (aspire)
if 1 & has_aspire %& streq(sg.type, 'par')
prompt
	Gw = Gtomo2_wtmex(sg, ig, 'pairs', {'strip_width', sg.d});
	yw = Gw * x;
	ys = G * x;
	max_percent_diff(yw, ys, 'sino Gtomo2_wtmex vs Gtomo2_strip')
	if im
		im pl 2 3, im(1, x), cbar
		im(1, ya, 'ya'), cbar
		im(2, ys, 'ys'), cbar
		im(3, yw, 'yw'), cbar
		im(4, ys-yw, 'ys-yw'), cbar
		xlabelf('%g%%', 100*nrms(ys(:), yw(:)))
		im(5, ys-ya, 'ys-ya'), cbar
		xlabelf('%g%%', 100*nrms(ys(:), ya(:)))
		im(6, yw-ya, 'yw-ya'), cbar
		xlabelf('%g%%', 100*nrms(yw(:), ya(:)))
	end
end


function [h, compUpAP, compUp] =  lfmComputeH3AP(gamma1_p, gamma1_m, sigma2, t1, ...
    t2, preFactor, mode)

% LFMCOMPUTEH3AP Helper function for computing part of the LFMAP kernel.
% FORMAT
% DESC computes a portion of the LFMAP kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG preFactor : precomputed constants.
% ARG mode: indicates the correct derivative.
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

% Evaluation of h

if nargout>1
    [compUpAP{1}, compUp{1}] = lfmapComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode);
    [compUpAP{2}, compUp{2}] = lfmapComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
    h = preFactor(1)*compUpAP{1} + preFactor(2)*compUpAP{2};
else
    h = preFactor(1)*lfmapComputeUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode) ...
        + preFactor(2)*lfmapComputeUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);
end


%% Misorientation Distribution Function
%
%% TODO: Please help to extend this section
% Let us consider the uncorrelated missorientation ODF corresponding to our
% model ODF.

mtexdata titanium

odf = calcDensity(ebsd.orientations)

%%

% the uncorrelated
mdf = calcMDF(odf)

%%

plotSection(mdf,'axisAngle')

%% Axis / Angle Distribution
% Then we can plot the distribution of the rotation axes of this
% missorientation ODF

plotAxisDistribution(mdf)

%%
% and the distribution of the missorientation angles and compare them to a
% uniform ODF

close all
plotAngleDistribution(mdf)
hold all
plotAngleDistribution(ebsd.CS,ebsd.CS)
hold off
legend('model ODF','uniform ODF')


function [X,Y,vals,labI]=mp_conic(optn,varargin)
% MP_CONIC  Conic projections
%           This function should not be used directly; instead it is
%           is accessed by various high-level functions named M_*.

% Rich Pawlowicz (rich@ocgy.ubc.ca) 2/Apr/1997
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
% Mathematical formulas for the projections and their inverses are taken from
%
%      Snyder, John P., Map Projections used by the US Geological Survey,
%      Geol. Surv. Bull. 1532, 2nd Edition, USGPO, Washington D.C., 1983.
%
% These are  conic projections with two standard parallels, useful
% for showing limited areas at mid-latitudes.
%    Albers equal-area - has an equal-area property
%    Lambert conformal - is conformal
%
%  7/6/99 - fixed tendency to re-define .ulongs if .clong set by user
%  3/4/02 - added error if parallels are equidistant from equator (i.e. not conic projection really)
%  24/10/08 - added ellipsoidal earth computations for lambert conformal conic projection
%  16/10/09 - added ellipsoidal earth computations for albers conic projection
%  06/03/17 - changed 'false_origin' to 'origin' as option name, also it
%             didn't work correctly with the 'normal' spheroid so this was
%             fixed, AND changed the default parallels so they were at 25%
%             and 75% limits instead of being a single parallel at the
%             center to prevent blowups with the albers ellipsoidal
%             projection AND made it work with SPHERE ellipsoid.

global MAP_PROJECTION MAP_VAR_LIST

MAP_ELLIP=mc_ellips;

name={'Albers Equal-Area Conic','Lambert Conformal Conic'};

pi180=pi/180;

switch optn

  case 'name'

     X=name;

  case {'usage','set'}

     m_names=fieldnames(MAP_ELLIP);

     X=char({['     ''' varargin{1} ''''],...
              '     <,''lon<gitude>'',[min max]>',...
              '     <,''lat<itude>'',[min max]>',...
              '     <,''clo<ngitude>'',value>',...
              '     <,''par<allels>'',[lat1 lat2]>',...
              '     <,''rec<tbox>'', ( ''on'' | ''off'' )>',...
              '     <,''ell<ipsoid>'', one of',...
    reshape(sprintf('         %6s',m_names{:}),15,length(m_names))',...
        '                               >',...
	      '     <,''ori<gin>'', [long lat]>'});

  case 'get'

     X=char([' Projection: ' MAP_PROJECTION.name '  (function: ' MAP_PROJECTION.routine ')'],...
            [' longitudes: ' num2str(MAP_VAR_LIST.ulongs) ' (centered at ' num2str(MAP_VAR_LIST.clong) ')'],...
            [' latitudes: ' num2str(MAP_VAR_LIST.ulats) ],...
            [' standard parallels: ' num2str(MAP_VAR_LIST.parallels) ],...
            [' Rectangular border: ' MAP_VAR_LIST.rectbox ],...
            [' ellipsoid: ' MAP_VAR_LIST.ellipsoid ],...
            [' origin: ' num2str(MAP_VAR_LIST.origin) ]);

  case 'initialize'

    MAP_VAR_LIST=[];
    MAP_PROJECTION.name=varargin{1};
    MAP_VAR_LIST.ulongs=[-180 -50];
    MAP_VAR_LIST.ulats=[10 85];
    MAP_VAR_LIST.parallels=NaN;
    MAP_VAR_LIST.clong=NaN;
    MAP_VAR_LIST.origin=NaN;
    MAP_VAR_LIST.rectbox='off';
    MAP_VAR_LIST.ellipsoid = 'normal';
    MAP_VAR_LIST.aussiemode=false;
    k=2;longs_def=0;
    while k<length(varargin)
      switch varargin{k}(1:3)
         case 'lon'
           MAP_VAR_LIST.ulongs=varargin{k+1}(:)';longs_def=1;
           if MAP_VAR_LIST.ulongs(1)>MAP_VAR_LIST.ulongs(2)
              MAP_VAR_LIST.ulongs=MAP_VAR_LIST.ulongs([2 1]);
           end
         case 'clo'
           MAP_VAR_LIST.clong=varargin{k+1};
         case 'lat'
           MAP_VAR_LIST.ulats=varargin{k+1}(:)';
         case 'par'
           MAP_VAR_LIST.parallels=varargin{k+1};
         case 'rec'
             switch lower(varargin{k+1}(1:2))
                 case {'on','bo'}
                    MAP_VAR_LIST.rectbox='on';
                 case 'of'
                    MAP_VAR_LIST.rectbox='on';
                 otherwise
                     error(['m_proj: Unrecognized box option: ' varargin{k+1}]);
             end
         case 'ell'
           MAP_VAR_LIST.ellipsoid=varargin{k+1};
         case 'ori'
           MAP_VAR_LIST.origin=varargin{k+1};
         case 'fal'
           error(' FALSE_ORIGIN option has been renamed to ORIGIN - Change your m_proj call! ');
	 case 'aus'  % aussiemode - my joke
	   if strcmp(varargin{k+1},'on')
	     MAP_VAR_LIST.aussiemode=true;
	   end
         otherwise
           disp(['Unknown option: ' varargin{k}]);
      end
       k=k+2;
    end
    if isnan(MAP_VAR_LIST.clong)
      if isnan(MAP_VAR_LIST.origin)
         MAP_VAR_LIST.clong=mean(MAP_VAR_LIST.ulongs);
      else
         MAP_VAR_LIST.clong=MAP_VAR_LIST.origin(1);
      end
    elseif  ~longs_def
        MAP_VAR_LIST.ulongs=MAP_VAR_LIST.clong+[-180 180];
    end
    if isnan(MAP_VAR_LIST.parallels), MAP_VAR_LIST.parallels=mean(MAP_VAR_LIST.ulats)*[1 1]+diff(MAP_VAR_LIST.ulats)*[-1/6 1/6]; end % change default mar/2017
    if isnan(MAP_VAR_LIST.origin), MAP_VAR_LIST.origin=[MAP_VAR_LIST.clong mean(MAP_VAR_LIST.parallels)]; end

    MAP_VAR_LIST.rlongs=MAP_VAR_LIST.ulongs*pi180;
    MAP_VAR_LIST.rlats=MAP_VAR_LIST.ulats*pi180;
    MAP_VAR_LIST.rparallels=MAP_VAR_LIST.parallels*pi180;
    MAP_VAR_LIST.rorigin=MAP_VAR_LIST.origin*pi180;

    MAP_VAR_LIST.ellip=getfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid);

    % These are constants used by the projection formulas

    switch MAP_PROJECTION.name
      case name(1)
        if MAP_VAR_LIST.ellip(2)==0 % spherical
           MAP_VAR_LIST.n=sum(sin(MAP_VAR_LIST.rparallels))/2;
           if MAP_VAR_LIST.n==0
               error('Your parallels are equidistant from the equator - use a cylindrical projection!');
           end
           MAP_VAR_LIST.C=cos(MAP_VAR_LIST.rparallels(1)).^2+2*MAP_VAR_LIST.n*sin(MAP_VAR_LIST.rparallels(1));
           MAP_VAR_LIST.rho0=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-2*MAP_VAR_LIST.n*sin( MAP_VAR_LIST.rorigin(2)   ))/MAP_VAR_LIST.n;
        else
 	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       m12=cos(MAP_VAR_LIST.rparallels)./sqrt(1-(e.*sin(MAP_VAR_LIST.rparallels)).^2);
	       q12=(1-e.^2)*(sin(MAP_VAR_LIST.rparallels)./(1-(e.*sin(MAP_VAR_LIST.rparallels)).^2) - ...
	            1./(2*e)*log((1-e.*sin(MAP_VAR_LIST.rparallels))./(1+e.*sin(MAP_VAR_LIST.rparallels))) );
	       q0= (1-e.^2)*(sin(MAP_VAR_LIST.rorigin(2))./(1-(e.*sin(MAP_VAR_LIST.rorigin(2))).^2) - ...
	            1./(2*e)*log((1-e.*sin(MAP_VAR_LIST.rorigin(2)))./(1+e.*sin(MAP_VAR_LIST.rorigin(2)))) );
           if diff( MAP_VAR_LIST.rparallels )==0
	          MAP_VAR_LIST.n=sin(MAP_VAR_LIST.rparallels(1));
           else
	          MAP_VAR_LIST.n=-diff(m12.^2)/diff(q12);
           end
	       MAP_VAR_LIST.C=m12(1).^2 + MAP_VAR_LIST.n.*q12(1);
	       MAP_VAR_LIST.rho0=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-MAP_VAR_LIST.n*q0)/MAP_VAR_LIST.n;
        end
      case name(2)
        if strcmp(MAP_VAR_LIST.ellipsoid,'normal')
           if diff(MAP_VAR_LIST.parallels)==0
             MAP_VAR_LIST.n=sin(MAP_VAR_LIST.rparallels(1));
           else
             MAP_VAR_LIST.n=-diff(log(cos(MAP_VAR_LIST.rparallels)))/diff(log(tan(MAP_VAR_LIST.rparallels/2+pi/4)));
           end
           MAP_VAR_LIST.F=cos(MAP_VAR_LIST.rparallels(1))/MAP_VAR_LIST.n* ...
                	  tan(pi/4+MAP_VAR_LIST.rparallels(1)/2).^MAP_VAR_LIST.n;
           MAP_VAR_LIST.rho0=MAP_VAR_LIST.F/tan(pi/4+ MAP_VAR_LIST.rorigin(2)/2).^MAP_VAR_LIST.n;
        else
 	      e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	      m12=cos(MAP_VAR_LIST.rparallels)./sqrt(1-(e.*sin(MAP_VAR_LIST.rparallels)).^2);
	      t12=tan(pi/4-MAP_VAR_LIST.rparallels/2)./( (1-e*sin(MAP_VAR_LIST.rparallels))./(1+e*sin(MAP_VAR_LIST.rparallels)) ).^(e/2);
	      tF=tan(pi/4-MAP_VAR_LIST.rorigin(2)/2)./( (1-e*sin(MAP_VAR_LIST.rorigin(2)))./(1+e*sin(MAP_VAR_LIST.rorigin(2))) ).^(e/2);
	      if diff(MAP_VAR_LIST.rparallels)==0
	         MAP_VAR_LIST.n=sin(MAP_VAR_LIST.rparallels(1));
          else
             MAP_VAR_LIST.n=diff(log(m12))/diff(log(t12));
          end
	      MAP_VAR_LIST.F=m12(1)/MAP_VAR_LIST.n/t12(1).^MAP_VAR_LIST.n;
	      MAP_VAR_LIST.rho0=MAP_VAR_LIST.ellip(1)*MAP_VAR_LIST.F*tF.^MAP_VAR_LIST.n;
        end
    end

    % check for a valid ellipsoid. if not, use the normalized sphere

    if ~isfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid)
       MAP_VAR_LIST.ellipsoid = 'normal';
    end

    % Get X/Y and (if we are in a box) update the lat/long limits.

    mu_util('xylimits');
    if strcmp(MAP_VAR_LIST.rectbox,'on'),  mu_util('lllimits'); end

  case 'll2xy'

    long=varargin{1};
    lat=varargin{2};
    vals=zeros(size(long));

    % Clip out-of-range values (lat/long box)

    if ~strcmp(MAP_VAR_LIST.rectbox,'on') && ~strcmp(varargin{4},'off')
        vals=vals | long<=MAP_VAR_LIST.longs(1)+eps*10 | long>=MAP_VAR_LIST.longs(2)-eps*10 | ...
	              lat<=MAP_VAR_LIST.lats(1)+eps*10 |   lat>=MAP_VAR_LIST.lats(2)-eps*10;
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(1),long<MAP_VAR_LIST.longs(1),lat);
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(2),long>MAP_VAR_LIST.longs(2),lat);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(1),lat<MAP_VAR_LIST.lats(1),long);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(2),lat>MAP_VAR_LIST.lats(2),long);
    end

    switch MAP_PROJECTION.name
      case name(1)
        if MAP_VAR_LIST.ellip(2)==0   % spherical
           rho=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-2*MAP_VAR_LIST.n*sin(lat*pi180))/MAP_VAR_LIST.n;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       q= (1-e.^2)*(sin(lat*pi180)./(1-(e.*sin(lat*pi180)).^2) - ...
	            1./(2*e)*log((1-e.*sin(lat*pi180))./(1+e.*sin(lat*pi180))) );
	       rho=MAP_VAR_LIST.ellip(1)*sqrt(MAP_VAR_LIST.C-MAP_VAR_LIST.n*q)/MAP_VAR_LIST.n;
        end
      case name(2)
        if strcmp(MAP_VAR_LIST.ellipsoid,'normal')
           lat(lat==-90)=-89.999; % Prevents /0 problems in next line
           rho=MAP_VAR_LIST.F ./ tan(pi/4+lat*pi180/2).^MAP_VAR_LIST.n;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       t=tan(pi/4-lat*pi180/2)./( (1-e*sin(lat*pi180))./(1+e*sin(lat*pi180)) ).^(e/2);
	       rho=MAP_VAR_LIST.ellip(1)*MAP_VAR_LIST.F*t.^MAP_VAR_LIST.n;
        end
    end
    theta=MAP_VAR_LIST.n*(long-MAP_VAR_LIST.origin(1))*pi180;

    X=real(rho.*sin(theta));
    Y=real(MAP_VAR_LIST.rho0-rho.*cos(theta));

    % Clip out-of-range values (rectangular box)

    if strcmp(MAP_VAR_LIST.rectbox,'on') && ~strcmp(varargin{4},'off')
        vals= vals | X<=MAP_VAR_LIST.xlims(1)+eps*10 | X>=MAP_VAR_LIST.xlims(2)-eps*10 | ...
                     Y<=MAP_VAR_LIST.ylims(1)+eps*10 | Y>=MAP_VAR_LIST.ylims(2)-eps*10;
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(1),X<MAP_VAR_LIST.xlims(1),Y);
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(2),X>MAP_VAR_LIST.xlims(2),Y);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(1),Y<MAP_VAR_LIST.ylims(1),X);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(2),Y>MAP_VAR_LIST.ylims(2),X);
    end
    if MAP_VAR_LIST.aussiemode, Y=-Y; X=-X; end

  case 'xy2ll'

    pi180=pi/180;

    if MAP_VAR_LIST.aussiemode, varargin{2}=-varargin{2}; varargin{1}=-varargin{1}; end
    switch MAP_PROJECTION.name
      case name(1)
        rho=sqrt(varargin{1}.^2+(MAP_VAR_LIST.rho0-varargin{2}).^2);
        theta=atan(varargin{1}./(MAP_VAR_LIST.rho0-varargin{2}));
        if  MAP_VAR_LIST.ellip(2)==0
           Y=asin((MAP_VAR_LIST.C-(rho*MAP_VAR_LIST.n/MAP_VAR_LIST.ellip(1)).^2)/(2*MAP_VAR_LIST.n))/pi180;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
           q=(MAP_VAR_LIST.C - (rho.*MAP_VAR_LIST.n/MAP_VAR_LIST.ellip(1)).^2)./MAP_VAR_LIST.n;
	       % Y is computed iteratively
	       Y=asin(q/2);
	       for k=1:4
	          Y=Y+(1-(e.*sin(Y)).^2).^2./(2*cos(Y)).*( q./(1-e.^2) - sin(Y)./(1-(e.*sin(Y)).^2) + ...
	             1./(2*e)*log( (1-e*sin(Y))./(1+e*sin(Y)) ) );
           end
	       Y=Y/pi180;
        end
        % The pole is an arc in this projection, so for points inside that
        % arc there is no inverse. If so, the math above can return complex
        % values - instead set these to NaN
        if ~isreal(Y)
           Y(imag(Y)>1e-7)=NaN;
           Y=real(Y);
        end

      case name(2)
        rho=sign(MAP_VAR_LIST.n)*sqrt(varargin{1}.^2+(MAP_VAR_LIST.rho0-varargin{2}).^2);
        theta=atan(varargin{1}./(MAP_VAR_LIST.rho0-varargin{2}));
	    if strcmp(MAP_VAR_LIST.ellipsoid,'normal')
           Y=(2*atan((MAP_VAR_LIST.F./rho).^(1/MAP_VAR_LIST.n))-pi/2)/pi180;
        else
	       e=sqrt(2*MAP_VAR_LIST.ellip(2)-MAP_VAR_LIST.ellip(2)^2);
	       tp=(rho./MAP_VAR_LIST.ellip(1)/MAP_VAR_LIST.F).^(1./MAP_VAR_LIST.n);
	       % Y is computed iteratively
	       Y=pi/2 - 2*atan(tp);
	       for k=1:4
	         Y=pi/2 - 2*atan(tp.*((1-e*sin(Y))./(1+e*sin(Y))).^(e/2) );
           end
	       Y=Y/pi180;
        end
    end
    % Clip out-of-range values (lat/long box)
    X=MAP_VAR_LIST.origin(1)+(theta/MAP_VAR_LIST.n)/pi180;

  case 'xgrid'

    [X,Y,vals,labI]=mu_util('xgrid',MAP_VAR_LIST.longs,MAP_VAR_LIST.lats,varargin{1},3,varargin{2:3});

  case 'ygrid'

    [X,Y,vals,labI]=mu_util('ygrid',MAP_VAR_LIST.lats,MAP_VAR_LIST.longs,varargin{1},31,varargin{2:3});

  case 'box'

    [X,Y]=mu_util('box',31);

end


function GMST=TAI2GAST(Jul1,Jul2,version,deltaT)
%%TAI2GAST Convert from international atomic time (TAI) to Greenwhich
%          apparent sidereal time (GAST), which is a measure of the
%          rotational direction of the Earth.
%
%INPUTS: Jul1,Jul2 Two parts of a pseudo-Julian date given in TAI. The
%                  units of the date are days. The full date is the sum of
%                  both terms. The date is broken into two parts to
%                  provide more bits of precision. It does not matter how
%                  the date is split.
%          version An optional integer specifying the theory to use for
%                  GMST. The theory chosen should be consistent with other
%                  values used in astronomical routines. Possible values
%                  are
%                  1982 Compute GAST ion accordance with the International
%                     Astronomical Union's (IAU's) 1982 model.
%                  2000 Compute GMST in line with IAU 2000 resolutions
%                     related to precession and nutation.
%                  2006 (The default if omitted) Compute GMST in line with
%                     IAU 2006 resolutions related to precession and
%                     nutation.
%           deltaT An optional parameter specifying the offset between TT
%                  and UT1 in seconds. If this parameter is omitted, then
%                  the value of the function getEOP will be used.
%
%OUTPUTS: GAST The Greenwhich apparent sideral time in radians.
%
%The function just calls TAI2TT and then TT2GAST.
%
%GAST is defined in Section 5.5.7 of [1].
%
%REFERENCES:
%[1] G. Petit and B. Luzum, IERS Conventions (2010), International Earth
%    Rotation and Reference Systems Service Std. 36, 2010.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=TAI2TT(Jul1,Jul2);

if(nargin==2)
    GMST=TT2GAST(Jul1,Jul2);
elseif(nargin==3)
    GMST=TT2GAST(Jul1,Jul2,version);
else
    GMST=TT2GAST(Jul1,Jul2,version,deltaT);
end

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function M = euclideanfactory(m, n)
% Returns a manifold struct to optimize over m-by-n matrices.
%
% function M = euclideanfactory(m, n)
%
% Returns M, a structure describing the Euclidean space of m-by-n matrices
% equipped with the standard Frobenius distance and associated trace inner
% product as a manifold for Manopt.

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%  July 5, 2013 (NB): added egred2rgrad, ehess2rhess, mat, vec, tangent.

    if ~exist('n', 'var') || isempty(n)
        n = 1;
    end

    M.name = @() sprintf('Euclidean space R^(%dx%d)', m, n);

    M.dim = @() m*n;

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @(x, y) norm(x-y, 'fro');

    M.typicaldist = @() sqrt(m*n);

    M.proj = @(x, d) d;

    M.egrad2rgrad = @(x, g) g;

    M.ehess2rhess = @(x, eg, eh, d) eh;

    M.tangent = M.proj;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d;
        else
            y = x + d;
        end
    end

    M.retr = M.exp;

	M.log = @(x, y) y-x;

    M.hash = @(x) ['z' hashmd5(x(:))];

    M.rand = @() randn(m, n);

    M.randvec = @randvec;
    function u = randvec(x) %#ok<INUSD>
        u = randn(m, n);
        u = u / norm(u, 'fro');
    end

    M.lincomb = @lincomb;
    function v = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>
        if nargin == 3
            v = a1*d1;
        elseif nargin == 5
            v = a1*d1 + a2*d2;
        else
            error('Bad usage of euclidean.lincomb');
        end
    end

    M.zerovec = @(x) zeros(m, n);

    M.transp = @(x1, x2, d) d;

    M.pairmean = @(x1, x2) .5*(x1+x2);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [m, n]);
    M.vecmatareisometries = @() true;

end


function jed = transition_to_jed_common ( )

%*****************************************************************************80
%
%% TRANSITION_TO_JED_COMMON returns the Common calendar transition as a JED.
%
%  Discussion:
%
%    In the Common calendar, the last moment of the Julian calendar was
%      11:59 pm, 4 October 1582 Julian/CE,
%      11:59 pm, 14 October 1582 Gregorian.
%    The first minute of the Gregorian calendar ended at
%      12:01 am, 5 October 1582 Julian,
%      12:01 am, 15 October 1582 Gregorian/CE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real JED, the Julian Ephemeris Date of the date.
%
  jed = 2299160.5;

  return
end


function [Yl,Yh,Yscale] = dtwavexfm2(X,nlevels,biort,qshift);

% Function to perform a n-level DTCWT-2D decompostion on a 2D matrix X
%
% [Yl,Yh,Yscale] = dtwavexfm2(X,nlevels,biort,qshift);
%
%     X -> 2D real matrix/Image
%
%     nlevels -> No. of levels of wavelet decomposition
%
%     biort ->  'antonini'   => Antonini 9,7 tap filters.
%               'legall'     => LeGall 5,3 tap filters.
%               'near_sym_a' => Near-Symmetric 5,7 tap filters.
%               'near_sym_b' => Near-Symmetric 13,19 tap filters.
%
%     qshift -> 'qshift_06' => Quarter Sample Shift Orthogonal (Q-Shift) 10,10 tap filters,
%                              (only 6,6 non-zero taps).
%               'qshift_a' =>  Q-shift 10,10 tap filters,
%                              (with 10,10 non-zero taps, unlike qshift_06).
%               'qshift_b' => Q-Shift 14,14 tap filters.
%               'qshift_c' => Q-Shift 16,16 tap filters.
%               'qshift_d' => Q-Shift 18,18 tap filters.
%
%
%     Yl     -> The real lowpass image from the final level
%     Yh     -> A cell array containing the 6 complex highpass subimages for each level.
%     Yscale -> This is an OPTIONAL output argument, that is a cell array containing
%               real lowpass coefficients for every scale.
%
%
% Example: [Yl,Yh] = dtwavexfm2(X,3,'near_sym_b','qshift_b');
% performs a 3-level transform on the real image X using the 13,19-tap filters
% for level 1 and the Q-shift 14-tap filters for levels >= 2.
%
% Nick Kingsbury and Cian Shaffrey
% Cambridge University, Sept 2001

if isstr(biort) & isstr(qshift)		%Check if the inputs are strings
   biort_exist = exist([biort '.mat']);
   qshift_exist = exist([qshift '.mat']);
   if biort_exist == 2 & qshift_exist == 2;        		%Check to see if the inputs exist as .mat files
      load (biort);
      load (qshift);
   else
      error('Please enter the correct names of the Biorthogonal or Q-Shift Filters, see help DTWAVEXFM2 for details.');
   end
else
   error('Please enter the names of the Biorthogonal or Q-Shift Filters as shown in help DTWAVEXFM2.');
end

orginal_size = size(X);

if ndims(X) >= 3;
   error(sprintf('The entered image is %dx%dx%d, please enter each image slice separately.',orginal_size(1),orginal_size(2),orginal_size(3)));
end

% The next few lines of code check to see if the image is odd in size, if so an extra ...
% row/column will be added to the bottom/right of the image
initial_row_extend = 0;  %initialise
initial_col_extend = 0;
if any(rem(orginal_size(1),2)), %if sx(1) is not divisable by 2 then we need to extend X by adding a row at the bottom
   X = [X; X(end,:)];           %Any further extension will be done in due course.
   initial_row_extend = 1;
end
if any(rem(orginal_size(2),2)), 	%if sx(2) is not divisable by 2 then we need to extend X by adding a col to the left
   X = [X X(:,end)];          %Any further extension will be done in due course.
   initial_col_extend = 1;
end
extended_size = size(X);

if nlevels == 0, return; end

%initialise
Yh=cell(nlevels,1);
if nargout == 3
   Yscale=cell(nlevels,1);   %this is only required if the user specifies a third output component.
end

S = [];
sx = size(X);
if nlevels >= 1,

   % Do odd top-level filters on cols.
   Lo = colfilter(X,h0o).';
   Hi = colfilter(X,h1o).';

   % Do odd top-level filters on rows.
   LoLo = colfilter(Lo,h0o).';			% LoLo
   Yh{1} = zeros([size(LoLo)/2  6]);
   Yh{1}(:,:,[1 6]) = q2c(colfilter(Hi,h0o).');			% Horizontal pair
   Yh{1}(:,:,[3 4]) = q2c(colfilter(Lo,h1o).');			% Vertical pair
   Yh{1}(:,:,[2 5]) = q2c(colfilter(Hi,h1o).');	      % Diagonal pair
   S = [ size(LoLo) ;S];
   if nargout == 3
      Yscale{1} = LoLo;
   end
end

if nlevels >= 2;
   for level = 2:nlevels;
      [row_size col_size] = size(LoLo);
      if any(rem(row_size,4)),		% Extend by 2 rows if no. of rows of LoLo are divisable by 4;
         LoLo = [LoLo(1,:); LoLo; LoLo(end,:)];
      end
      if any(rem(col_size,4)),		% Extend by 2 cols if no. of cols of LoLo are divisable by 4;
         LoLo = [LoLo(:,1)  LoLo  LoLo(:,end)];
      end

      % Do even Qshift filters on rows.
      Lo = coldfilt(LoLo,h0b,h0a).';
      Hi = coldfilt(LoLo,h1b,h1a).';

      % Do even Qshift filters on columns.
      LoLo = coldfilt(Lo,h0b,h0a).';	%LoLo
      Yh{level} = zeros([size(LoLo)/2  6]);
      Yh{level}(:,:,[1 6]) = q2c(coldfilt(Hi,h0b,h0a).');	% Horizontal
      Yh{level}(:,:,[3 4]) = q2c(coldfilt(Lo,h1b,h1a).');	% Vertical
      Yh{level}(:,:,[2 5]) = q2c(coldfilt(Hi,h1b,h1a).');	% Diagonal
      S = [ size(LoLo) ;S];
      if nargout == 3
         Yscale{level} = LoLo;
      end
   end
end

Yl = LoLo;

if initial_row_extend == 1 & initial_col_extend == 1;
   warning(sprintf(' \r\r The image entered is now a %dx%d NOT a %dx%d \r The bottom row and rightmost column have been duplicated, prior to decomposition. \r\r ',...
      extended_size(1),extended_size(2),orginal_size(1),orginal_size(2)));
end

if initial_row_extend == 1 ;
   warning(sprintf(' \r\r The image entered is now a %dx%d NOT a %dx%d \r Row number %d has been duplicated, and added to the bottom of the image, prior to decomposition. \r\r',...
      extended_size(1),extended_size(2),orginal_size(1),orginal_size(2),orginal_size(1)));
end

if initial_col_extend == 1;
   warning(sprintf(' \r\r The image entered is now a %dx%d NOT a %dx%d \r Col number %d has been duplicated, and added to the right of the image, prior to decomposition. \r\r',...
      extended_size(1),extended_size(2),orginal_size(1),orginal_size(2),orginal_size(2)));
end
return

%==========================================================================================
%						**********  	INTERNAL FUNCTION    **********
%==========================================================================================

function z = q2c(y)

% function z = q2c(y)
% Convert from quads in y to complex numbers in z.

sy = size(y);
t1 = 1:2:sy(1); t2 = 1:2:sy(2);
j2 = sqrt([0.5 -0.5]);

% Arrange pixels from the corners of the quads into
% 2 subimages of alternate real and imag pixels.
%  a----b
%  |    |
%  |    |
%  c----d

% Combine (a,b) and (d,c) to form two complex subimages.
p = y(t1,t2)*j2(1) + y(t1,t2+1)*j2(2);     % p = (a + jb) / sqrt(2)
q = y(t1+1,t2+1)*j2(1) - y(t1+1,t2)*j2(2); % q = (d - jc) / sqrt(2)

% Form the 2 subbands in z.
z = cat(3,p-q,p+q);

return


function p = cycle_to_perm ( n, ncycle, t, index )

%*****************************************************************************80
%
%% CYCLE_TO_PERM converts a permutation from cycle to array form.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of items permuted.
%    N must be positive.
%
%    Input, integer NCYCLE, the number of cycles.
%    1 <= NCYCLE <= N.
%
%    Input, integer T(N), INDEX(NCYCLE), describes the permutation
%    as a collection of NCYCLE cycles.  The first cycle is
%    T(1) -> T(2) -> ... -> T(INDEX(1)) -> T(1).
%
%    Output, integer P(N), describes the permutation using a
%    single array.  For each index I, I -> P(I).
%

%
%  Check.
%
  ierror = cycle_check ( n, ncycle, t, index );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CYCLE_TO_PERM - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'CYCLE_TO_PERM - Fatal error!' );
  end

  jhi = 0;

  for i = 1 : ncycle

    jlo = jhi + 1;
    jhi = jhi + index(i);

    for j = jlo : jhi

      if ( j < jhi )
        p(t(j)) = t(j+1);
      else
        p(t(j)) = t(jlo);
      end

    end

  end

  return
end


%%*************************************************************************
%% mybicgstab
%%
%% [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit)
%%
%% iterate on  bb - (M1)*AA*x
%%
%% r = b-A*xtrue;
%%
%%*************************************************************************

function [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit,printlevel)

N = length(b);
if (nargin < 6); printlevel = 1; end
if (nargin < 5) || isempty(maxit); maxit = max(30,length(A.mat22)); end;
if (nargin < 4) || isempty(tol); tol = 1e-10; end;
tolb = min(1e-4,tol*norm(b));
flag = 1;

x = zeros(N,1);
if (norm(x))
    if isstruct(A); r = b-matvec(A,x); else r = b-mexMatvec(A,x); end;
else
    r =b;
end
err = norm(r); resnrm(1) = err;  minresnrm = err; xx = x;
%%if (err < 1e-3*tolb); return; end

omega  = 1.0;
r_tld = r;
%%
%%
%%
breakyes = 0;
smtol = 1e-40;
for iter = 1:maxit,

    rho   = (r_tld'*r);
    if (abs(rho) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        breakyes = 1;
        break;
    end
    if (iter > 1)
        beta  = (rho/rho_1)* (alp/omega);
        p = r + beta*(p - omega*v);
    else
        p = r;
    end
    p_hat = precond(A,M1,p);
    if isstruct(A); v = matvec(A,p_hat); else v = mexMatvec(A,p_hat); end;
    alp = rho / (r_tld'*v);
    s = r - alp*v;
    %%
    s_hat = precond(A,M1,s);
    if isstruct(A); t = matvec(A,s_hat); else t = mexMatvec(A,s_hat); end;
    omega = (t'*s) / (t'*t);
    x = x + alp*p_hat + omega*s_hat;
    r = s - omega*t;
    rho_1 = rho;
    %%
    %% check convergence
    %%
    err = norm(r); resnrm(iter+1) = err; %#ok
    if (err < minresnrm);
        xx = x; minresnrm = err;
    end
    if (err < tolb)
        break;
    end
    if (err > 10*minresnrm)
        if (printlevel); fprintf('^'); end
        breakyes = 2;
        break;
    end
    if (abs(omega) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end
        breakyes = 1;
        break;
    end
end
if (~breakyes) && (printlevel >=3); fprintf(' '); end
%%
%%*************************************************************************
%%*************************************************************************
%% precond:
%%*************************************************************************

function Mx = precond(A,L,x)

m = L.matdim; m2 = length(x)-m;
Mx = zeros(length(x),1);

for iter = 1
    if norm(Mx); r = x - matvec(A,Mx); else r = x; end
    if (m2 > 0)
        r1 = full(r(1:m));
    else
        r1 = full(r);
    end
    if (m2 > 0)
        r2 = r(m+1:m+m2);
        w = linsysolvefun(L,r1);
        z = mexMatvec(A.mat12,w,1) - r2;
        z = L.Mu \ (L.Ml \ (L.Mp*z));
        r1 = r1 - mexMatvec(A.mat12,z);
    end
    d = linsysolvefun(L,r1);
    if (m2 > 0)
        d = [d; z]; %#ok
    end
    Mx = Mx + d;
end
%%*************************************************************************
%%*************************************************************************
%% matvec: matrix-vector multiply.
%% matrix = [A.mat11, A.mat12; A.mat12', A.mat22]
%%*************************************************************************

function Ax = matvec(A,x)

m = length(A.mat11); m2 = length(x)-m;
if issparse(x); x = full(x); end
if (m2 > 0)
    x1 = x(1:m);
else
    x1 = x;
end
Ax = mexMatvec(A.mat11,x1);
if (m2 > 0)
    x2 = x(m+1:m+m2);
    Ax = Ax + mexMatvec(A.mat12,x2);
    Ax2 = mexMatvec(A.mat12,x1,1) + mexMatvec(A.mat22,x2);
    Ax = [full(Ax); full(Ax2)];
end
%%*************************************************************************


function h = hs71H(x,lambda)

h = [ 2*x(4)             x(4)   x(4)   2*x(1)+x(2)+x(3);
      x(4)               0      0   x(1);
      x(4)               0      0   x(1);
      2*x(1)+x(2)+x(3)  x(1)  x(1)  0 ];

h = h + lambda.ineqnonlin*-[    0      x(3)*x(4) x(2)*x(4) x(2)*x(3);
                            x(3)*x(4)     0     x(1)*x(4) x(1)*x(3);
                            x(2)*x(4) x(1)*x(4)     0     x(1)*x(2);
                            x(2)*x(3) x(1)*x(3) x(1)*x(2)     0  ];

h = h + lambda.eqnonlin * diag([2 2 2 2]);


function variance = normal_01_variance ( )

%*****************************************************************************80
%
%% NORMAL_01_VARIANCE returns the variance of the Normal 01 PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = 1.0;

  return
end


function bessel_k0_int_values_test ( )

%*****************************************************************************80
%
%% BESSEL_KO_INT_VALUES_TEST demonstrates the use of BESSEL_KO_INT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_KO_INT_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_K0_INT_VALUES stores values of \n' );
  fprintf ( 1, '  the integral of the Bessel function K0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_k0_int_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function [E,J]=SynthMeasWatsonSHCylSingleRadIsoV_GPD(x, protocol, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Signal approximation: Gaussian phase distribution.
% Notes: This version includes an isotropic diffusion compartment with its own
% diffusivity.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadIsoV_GPD(x, protocol, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the hindered diffusivity outside the cylinders in perpendicular directions.
% x(4) is the radius of the cylinders.
% x(5) is the concentration parameter of the Watson's distribution.
% x(6) is the volume fraction of the isotropic compartment.
% x(7) is the diffusivity of the isotropic compartment.
%
% protocol is the object containing the acquisition protocol.
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

fiso = x(6);
dIso = x(7);

% Call the model with no isotropic component to get the anisotropic component.
if(nargout == 1)
    Eaniso=SynthMeasWatsonSHCylSingleRadGPD(x, protocol, fibredir, roots);
    Eiso = SynthMeasIsoGPD(dIso, protocol);
else
    [Eaniso,Janiso]=SynthMeasWatsonSHCylSingleRadGPD(x, protocol, fibredir, roots);
    [Eiso, Jiso] = SynthMeasIsoGPD(dIso, protocol);
end

E = (1-fiso)*Eaniso + fiso*Eiso;

if(nargout>1)

    % Update with anisotropic component.
    J = Janiso*(1-fiso);

    % Add derivatives wrt isotropic fraction.
    J(:,6) = Eiso - Eaniso;

    % Add entry for dIso
    J(:,7) = fiso*Jiso;
end


function [Mz_z,Mz_xy,F,ref_eff,Mx_xy,My_xy]=simRf(rf,rephase_factor,prephase_factor)
%simRf Simulate an RF pulse with the given pulse shape.
%   [Mz_z,Mz_xy,F,ref_eff,Mx_xy,My_xy]=simRf(pulse,prephase_factor,rephase_factor)
%   Performs a rapid RF pulse simulation based on the rotation formalism.
%   The algorithm is optimized by using quaternions to represent rotations.
%   The compulsory parameter 'rf' is the Pulseq RF pulse. Optional
%   parameter 'rephase_factor' is needed in several cases e.g. to correclty
%   visualize the phase of the magnetization for slice-selective
%   excitation. Another optional parameter 'prephase_factor' is an
%   experimental parameter useful for simulating refocusing pulses or
%   spoiling needed.
%   Return values:
%     Mz_z,Mz_xy:  z and xy comnponents of the magnetisation after the pulse
%                  assuming the unit magnetization was aligned with z before
%                  the pulse. Useful for assessing excitation RF pulses.
%     F:           frequency axis in Hz
%     ref_eff:     Refocusing efficiency of the pulse as a complex value.
%                  Magnitude of ref_eff seems to closely follow Mz_z. Phase
%                  of ref_eff is related to the effective phase of the RF
%                  pulse, e.g. the axis of the planar flip.
%     Mx_xy,My_xy: xy magnetizations after the RF pulse assuming the unit
%                  magnetization was aligned with x or y axis prior to the
%                  pulse, respectively. Useful for detailed analyses of
%                  refocusing pulses.
%
%   The implementation was inspired by the example by Dr. Tony Stoecker
%   (https://github.com/stoeckert/mr-simu-example-ismrm19)
%   The algorithm was rewritten to quaternions and vectorized for
%   performance by MZ
%

bw_mul=4;    % simulation bandwidth (multiplier of the pulse bandwidth)
df=1;        % spectral resolution [Hz]
dt=10e-6;    % (re-)sampling interval

if nargin < 2
    if isfield(rf,'use') && strcmp(rf.use,'refocusing')
        rephase_factor = 0;
    else
        rephase_factor = -(rf.shape_dur-mr.calcRfCenter(rf))/rf.shape_dur;
    end
end

if nargin < 3
    prephase_factor = 0;
end

[bw,f0,spectrum,FF,rfs,tt]=mr.calcRfBandwidth(rf,0.5,df*10,dt);

T     = (1:round(rf.shape_dur/dt))*dt-0.5*dt;                           % timesteps axis [s]
F     = 2*pi*linspace(f0-bw_mul*bw/2,f0+bw_mul*bw/2,bw/df)';               % offset frequencies [rad/s]

shapea = interp1(rf.t, 2*pi*rf.signal.*exp(1i*(rf.phaseOffset+2*pi*rf.freqOffset*rf.t)),T,'linear',0);

% intialize result vectors
M_ROT=zeros(size(F));
Z_ROT=zeros(size(F));
sf=size(F);
q=zeros(sf(1),4);
q(:,1)=1; % init rotation quaternions

% prephaser / left spoiler
W = -F*dt*length(T)*prephase_factor;     % effective field rotation angle
Q = [cos(W/2) zeros(sf) zeros(sf) sin(W/2)];
q=quat_multiply(q,Q);

% RF pulse simulation
for j=1:length(T)
    W = -dt*sqrt(abs(shapea(j))^2+F.^2); % effective field rotation angles
    n = dt * [real(shapea(j))*ones(sf) imag(shapea(j))*ones(sf) F]./abs(W); % effective field rotation axes
    Q = [cos(W/2) sin(W/2).*n];
    q=quat_multiply(q,Q);
end

% rephaser / right spoiler / refocusing pulse
W = -F*dt*length(T)*rephase_factor;     % effective field rotation angle
Q = [cos(W/2) zeros(sf) zeros(sf) sin(W/2)];
q=quat_multiply(q,Q);

% export results
F=F/(2*pi);
m=zeros(sf(1),4);

% excitation: start with M0=M_z
m(:,4)=1;
m0rf=quat_multiply(quat_conj(q),quat_multiply(m,q));
Mz_z=m0rf(:,4);
Mz_xy=m0rf(:,2)+1i*m0rf(:,3);

% refocusing: start both with M0=M_x and them M0=M_y
m=zeros(sf(1),4);
m(:,2)=1;
Mx_xy=quat_multiply(quat_conj(q),quat_multiply(m,q));
Mx_xy=Mx_xy(:,2)+1i*Mx_xy(:,3);
m=zeros(sf(1),4);
m(:,3)=1;
My_xy=quat_multiply(quat_conj(q),quat_multiply(m,q));
My_xy=My_xy(:,2)+1i*My_xy(:,3);
ref_eff=(Mx_xy+My_xy*1i)/2;
end

function qout = quat_multiply( q, r )
%  quat_multiply: Calculate the product of two quaternions.

% Calculate vector portion of quaternion product
% vec = s1*v2 + s2*v1 + cross(v1,v2)
vec = [q(:,1).*r(:,2) q(:,1).*r(:,3) q(:,1).*r(:,4)] + ...
         [r(:,1).*q(:,2) r(:,1).*q(:,3) r(:,1).*q(:,4)]+...
         [ q(:,3).*r(:,4)-q(:,4).*r(:,3) ...
           q(:,4).*r(:,2)-q(:,2).*r(:,4) ...
           q(:,2).*r(:,3)-q(:,3).*r(:,2)];

% Calculate scalar portion of quaternion product
% scalar = s1*s2 - dot(v1,v2)
scalar = q(:,1).*r(:,1) - q(:,2).*r(:,2) - ...
             q(:,3).*r(:,3) - q(:,4).*r(:,4);

qout = [scalar  vec];
end

function q = quat_conj( q )
%  quat_conj Calculate the conjugate of a quaternion.
q(:,2:4) = -q(:,2:4);
end


% Version 1.000
%
% Code provided by Ruslan Salakhutdinov and Geoff Hinton
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own risk.

function [f, df] = CG_MNIST(VV,Dim,XX);

l1 = Dim(1);
l2 = Dim(2);
l3 = Dim(3);
l4= Dim(4);
l5= Dim(5);
l6= Dim(6);
l7= Dim(7);
l8= Dim(8);
l9= Dim(9);
N = size(XX,1);

% Do decomversion.
 w1 = reshape(VV(1:(l1+1)*l2),l1+1,l2);
 xxx = (l1+1)*l2;
 w2 = reshape(VV(xxx+1:xxx+(l2+1)*l3),l2+1,l3);
 xxx = xxx+(l2+1)*l3;
 w3 = reshape(VV(xxx+1:xxx+(l3+1)*l4),l3+1,l4);
 xxx = xxx+(l3+1)*l4;
 w4 = reshape(VV(xxx+1:xxx+(l4+1)*l5),l4+1,l5);
 xxx = xxx+(l4+1)*l5;
 w5 = reshape(VV(xxx+1:xxx+(l5+1)*l6),l5+1,l6);
 xxx = xxx+(l5+1)*l6;
 w6 = reshape(VV(xxx+1:xxx+(l6+1)*l7),l6+1,l7);
 xxx = xxx+(l6+1)*l7;
 w7 = reshape(VV(xxx+1:xxx+(l7+1)*l8),l7+1,l8);
 xxx = xxx+(l7+1)*l8;
 w8 = reshape(VV(xxx+1:xxx+(l8+1)*l9),l8+1,l9);

  XX = [XX ones(N,1)];
  w1probs = 1./(1 + exp(-XX*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];
  w4probs = w3probs*w4; w4probs = [w4probs  ones(N,1)];
  w5probs = 1./(1 + exp(-w4probs*w5)); w5probs = [w5probs  ones(N,1)];
  w6probs = 1./(1 + exp(-w5probs*w6)); w6probs = [w6probs  ones(N,1)];
  w7probs = 1./(1 + exp(-w6probs*w7)); w7probs = [w7probs  ones(N,1)];
  XXout = 1./(1 + exp(-w7probs*w8));

f = -1/N*sum(sum( XX(:,1:end-1).*log(XXout) + (1-XX(:,1:end-1)).*log(1-XXout)));
IO = 1/N*(XXout-XX(:,1:end-1));
Ix8=IO;
dw8 =  w7probs'*Ix8;

Ix7 = (Ix8*w8').*w7probs.*(1-w7probs);
Ix7 = Ix7(:,1:end-1);
dw7 =  w6probs'*Ix7;

Ix6 = (Ix7*w7').*w6probs.*(1-w6probs);
Ix6 = Ix6(:,1:end-1);
dw6 =  w5probs'*Ix6;

Ix5 = (Ix6*w6').*w5probs.*(1-w5probs);
Ix5 = Ix5(:,1:end-1);
dw5 =  w4probs'*Ix5;

Ix4 = (Ix5*w5');
Ix4 = Ix4(:,1:end-1);
dw4 =  w3probs'*Ix4;

Ix3 = (Ix4*w4').*w3probs.*(1-w3probs);
Ix3 = Ix3(:,1:end-1);
dw3 =  w2probs'*Ix3;

Ix2 = (Ix3*w3').*w2probs.*(1-w2probs);
Ix2 = Ix2(:,1:end-1);
dw2 =  w1probs'*Ix2;

Ix1 = (Ix2*w2').*w1probs.*(1-w1probs);
Ix1 = Ix1(:,1:end-1);
dw1 =  XX'*Ix1;

df = [dw1(:)' dw2(:)' dw3(:)' dw4(:)' dw5(:)' dw6(:)'  dw7(:)'  dw8(:)'  ]';


function [E,J]=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD_B0(x, protocol, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Signal approximation: Gaussian phase distribution.
% Notes: This version estimates the hindered diffusivity from the free diffusivity
% and packing density using Szafer et al's tortuosity model for randomly
% packed cylinders.
% This version includes an isotropic diffusion compartment with its own
% diffusivity.
% This version includes a stationary water compartment.
% Includes a free parameter for the measurement at b=0.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD_B0(x, protocol, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the radius of the cylinders.
% x(4) is the concentration parameter of the Watson's distribution.
% x(5) is the volume fraction of the isotropic compartment.
% x(6) is the diffusivity of the isotropic compartment.
% x(7) is the volume fraction of the isotropic restriction.
% x(8) is the measurement at b=0.
%
% protocol is the object containing the acquisition protocol.
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

S0 = x(8);

% Call the other function to get normalized measurements.
if(nargout == 1)
    Enorm=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD(x, protocol, fibredir, roots);
else
   [Enorm,Jnorm]=SynthMeasWatsonSHCylSingleRadTortIsoVIsoDot_GPD(x, protocol, fibredir, roots);
end

E = Enorm*S0;

if(nargout>1)
    J = Jnorm*S0;
    J(:,8) = Enorm;
end


function op = prox_l1pos( q )
%PROX_L1POS    L1 norm, restricted to x >= 0
%    OP = PROX_L1( q ) implements the nonsmooth function
%        OP(X) = norm(q.*X,1) + indicator_{ X >= 0 }
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be a positive real scalar (or must be same size as X).

% New in v1.0d

if nargin == 0,
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) ||  any( q(:) < 0 ) || all(q(:)==0) %|| numel( q ) ~= 1
	error( 'Argument must be positive.' );
end

op = tfocs_prox( @(x)f(x,q), @(x,t)prox_f(x,t,q), 'vector');
end

function v = f(x,q)
    if any( x(:) < 0 )
        v = Inf;
    elseif isscalar(q)
        v = q*sum( x(:) );
    else
        v = sum( q(:).*x(:) );
    end
end

% The proximity operator is a simplified version of shrinkage:
function x = prox_f(x,t,q)
    x   = max( 0, x - t*q );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function [soln,eqn,info] = StokesP2P0(node,elem,bdFlag,pde,option)
%% STOKESP2P0 Stokes equation: P2-P0 elements.
%
%   [soln,eqn,info] = STOKESP2P0(node,elem,bdFlag,pde) use quadratic and piceswise
%   constant elements to approximate velocity u and pressure p, repectively.
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%   with
%       Dirichlet boundary condition        u = g_D  on \Gamma_D,
%       Neumann boundary condition du/dn - np = g_N  on \Gamma_N.
%
%   It is a choice of option.fem in Stokes. Please read Stokes for more
%   information on the input and output.
%
% See also Stokes, Poisson, StokesP2P1
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end

%% Construct Data Structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1);  Nu = N+size(edge,1);   Np = NT;

t = cputime;
%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix for Laplace operator
% generate sparse pattern
ii = zeros(21*NT,1); jj = zeros(21*NT,1);
index = 0;
for i = 1:6
    for j = i:6
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        index = index + NT;
    end
end
% quadrature points
if ~isfield(pde,'nu'), pde.nu = []; end
if ~isfield(option,'quadorder')
    % constant viscosity
    option.quadorder = 2;        % default order
    if ~isempty(pde.nu) && isnumeric(pde.nu) % numerical viscosity
        option.quadorder = 3;    % exact for linear diffusion coefficient
    end
end
[lambda, w] = quadpts(option.quadorder);
nQuad = size(lambda,1);
% compute non-zeros
sA = zeros(21*NT,nQuad);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    index = 0;
    for i = 1:6
        for j = i:6
            Aij = 0;
            if isempty(pde.nu) || isnumeric(pde.nu)
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
            else
                pxy = lambda(p,1)*node(elem(:,1),:) ...
                    + lambda(p,2)*node(elem(:,2),:) ...
                    + lambda(p,3)*node(elem(:,3),:);
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
            end
            if ~isempty(pde.nu) && (pde.nu~=1)
                Aij = pde.nu*Aij;
            end
            Aij = Aij.*area;
            sA(index+1:index+NT,p) = Aij;
            index = index + NT;
        end
    end
end
sA = sum(sA,2);
% assemble the matrix
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Nu,Nu);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Nu,Nu);
A = A + AU + AU';
A = blkdiag(A,A);
clear Aij ii jj sA Dphip

%% Assemble matrix for divergence operator
% Since Dphi is linear, 1-pt quadrature is exact for the integral
% int(div(phi)). We evaluate each basis at the barycenter.
Dphic(:,:,6) = 4/3*(Dlambda(:,:,1) + Dlambda(:,:,2));
Dphic(:,:,1) = 1/3*Dlambda(:,:,1); % (4*lambda(p,1)-1).*Dlambda(:,:,1);
Dphic(:,:,2) = 1/3*Dlambda(:,:,2);
Dphic(:,:,3) = 1/3*Dlambda(:,:,3);
Dphic(:,:,4) = 4/3*(Dlambda(:,:,2) + Dlambda(:,:,3));
Dphic(:,:,5) = 4/3*(Dlambda(:,:,3) + Dlambda(:,:,1));
clear Dlambda
% divergence matrix
Dx = sparse(Np,Nu);
Dy = sparse(Np,Nu);
for i = 1:6
    Dx = Dx + sparse(1:NT,double(elem2dof(:,i)),Dphic(:,1,i).*area,Np,Nu);
    Dy = Dy + sparse(1:NT,double(elem2dof(:,i)),Dphic(:,2,i).*area,Np,Nu);
end
B = -[Dx Dy]; %#ok<*NASGU>
clear Dphi Dx Dy

%% Assemble right hand side
f1 = zeros(Nu,1);
f2 = zeros(Nu,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,weight] = quadpts(option.fquadorder);
    % basis values at quadrature points
    phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
    phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
    phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
    phi(:,4) = 4*lambda(:,2).*lambda(:,3);
    phi(:,5) = 4*lambda(:,3).*lambda(:,1);
    nQuad = size(lambda,1);
    ft1 = zeros(NT,6);
    ft2 = zeros(NT,6);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        % function values at quadrature points
        fp = pde.f(pxy);
        for j = 1:6
            ft1(:,j) = ft1(:,j) + fp(:,1).*phi(p,j)*weight(p);
            ft2(:,j) = ft2(:,j) + fp(:,2).*phi(p,j)*weight(p);
        end
    end
    ft1 = ft1.*repmat(area,1,6);
    ft2 = ft2.*repmat(area,1,6);
    f1 = accumarray(elem2dof(:),ft1(:),[Nu 1]);
    f2 = accumarray(elem2dof(:),ft2(:),[Nu 1]);
end

%% Boundary condition
[AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesP2P0;

%% Record assembeling time
assembleTime = cputime - t;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(ufreeDof), return; end
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if length(f)+length(g) <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else          % Multigrid-type  solver for large size systems
        option.solver = 'asmg';
    end
end
solver = option.solver;

%% Solver
switch solver
    case 'direct'
        t = cputime;
        bigA = [AD, BD'; ...
                BD, sparse(Np,Np)];
        bigF = [f; g];
        bigu = [u; p];
        bigFreeDof = [ufreeDof; 2*Nu+pDof];
        bigu(bigFreeDof) = bigA(bigFreeDof,bigFreeDof)\bigF(bigFreeDof);
        u = bigu(1:2*Nu);
        p = bigu(2*Nu+1:end);
        residual = norm(bigF - bigA*bigu);
        info = struct('solverTime',cputime - t,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'mg'
        option.solver  = 'WCYCLE';
        [u(ufreeDof),p,info] = mgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                        u(ufreeDof),p,elem,ufreeDof,option);
    case 'asmg'
        [u(ufreeDof),p,info] = asmgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                          u,p,node,elem,bdFlag,ufreeDof,option);
end

%% Post-process
if length(pDof) ~= Np % p is unique up to a constant
    % impose the condition int(p)=0
    c = sum(p.*area)/sum(area);
    p = p - c;
end

%% Output
soln = struct('u',u,'p',p);
eqn = struct('A',AD,'B',BD,'Lap',A,'f',f,'g',g,...
             'edge',edge,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdStokesP2P0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesP2P0
    %% Boundary condition of Stokes equation: P2-P0 elements

    %% Initial set up
%     f = [f1; f2];    % set in Neumann boundary condition
    g = zeros(Np,1);
    u = zeros(2*Nu,1);
    p = zeros(Np,1);
    ufreeDof = (1:Nu)';
    pDof = (1:Np)';

    if ~exist('bdFlag','var'), bdFlag = []; end
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Find Dirichlet dof and modify the matrix
    % Find Dirichlet boundary dof: fixedDof and pDof
    isFixedDof = false(Nu,1);
    if ~isempty(bdFlag)       % case: bdFlag is not empty
        elem2edge = elem2dof(:,4:6)-N;
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;   % nodes of all D-edges
        isFixedDof(N + find(isDirichlet')) = true;% dof on D-edges
        fixedDof = find(isFixedDof);
        ufreeDof = find(~isFixedDof);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        fixedDof = bdDof;
        isFixedDof(fixedDof) = true;
        ufreeDof = find(~isFixedDof);
    end
    if isempty(fixedDof) % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        fixedDof = 1;
        ufreeDof = 2:Nu;    % eliminate the kernel by enforcing u(1) = 0;
    end

    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedDof,fixedDof)=I, AD(fixedDof,ufreeDof)=0, AD(ufreeDof,fixedDof)=0.
    % BD(:,fixedDof) = 0 and thus BD'(fixedDof,:) = 0.
    bdidx = zeros(2*Nu,1);
    bdidx(fixedDof) = 1;
    bdidx(Nu+fixedDof) = 1;
    Tbd = spdiags(bdidx,0,2*Nu,2*Nu);
    T = spdiags(1-bdidx,0,2*Nu,2*Nu);
    AD = T*A*T + Tbd;
    BD = B*T;

    %% Part 2: Find boundary edges and modify the right hand side f and g
    % Find boundary edges: Neumann and Robin
    Neumann = []; Robin = []; %#ok<*NASGU>
    if ~isempty(bdFlag)
        isNeumann(elem2edge((bdFlag(:)==2)|(bdFlag(:) == 3))) = true;
        isRobin(elem2edge(bdFlag(:)==3)) = true;
        Neumannidx = find(isNeumann);
        Neumann   = edge(isNeumann,:);
        Robin     = edge(isRobin,:);
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        Neumann = edge(bdDof>N,:);
        if ~isempty(pde.g_R)
            Robin = Neumann;
        end
    end

    % Neumann boundary condition
    if ~isempty(pde.g_N) && ~isempty(Neumann) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        [lambda,w] = quadpts1(3);
        nQuad = size(lambda,1);
        % quadratic bases (1---3---2)
        bdphi(:,1) = (2*lambda(:,1)-1).*lambda(:,1);
        bdphi(:,2) = (2*lambda(:,2)-1).*lambda(:,2);
        bdphi(:,3) = 4*lambda(:,1).*lambda(:,2);
        % length of edge
        ve = node(Neumann(:,1),:) - node(Neumann(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        % update RHS
        gex = zeros(size(Neumann,1),2);
        gey = zeros(size(Neumann,1),2);
        for pp = 1:nQuad
            pxy = lambda(pp,1)*node(Neumann(:,1),:)+lambda(pp,2)*node(Neumann(:,2),:);
            gp = pde.g_N(pxy);
            gex(:,1) = gex(:,1) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,1);
            gex(:,2) = gex(:,2) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,2);
            gey(:,1) = gey(:,1) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,1);
            gey(:,2) = gey(:,2) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,2);
            f1(N+Neumannidx) = f1(N+Neumannidx) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,3); % interior bubble
            f2(N+Neumannidx) = f2(N+Neumannidx) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,3); % interior bubble
        end
        f1(1:N) = f1(1:N) + accumarray(Neumann(:), gex(:),[N,1]);
        f2(1:N) = f2(1:N) + accumarray(Neumann(:), gey(:),[N,1]);
    end
    f = [f1; f2];
    % The case non-empty Neumann but g_N=[] corresponds to the zero flux
    % boundary condition on Neumann edges and no modification is needed.

    % Dirichlet boundary conditions
    if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
        u1 = zeros(Nu,1);
        u2 = zeros(Nu,1);
        idx = (fixedDof > N);              % index of edge dof
        uD = pde.g_D(node(fixedDof(~idx),:));  % bd value at vertex dofs
        u1(fixedDof(~idx)) = uD(:,1);
        u2(fixedDof(~idx)) = uD(:,2);
        bdEdgeIdx = fixedDof(idx)-N;
        bdEdgeMid = (node(edge(bdEdgeIdx,1),:)+node(edge(bdEdgeIdx,2),:))/2;
        uD = pde.g_D(bdEdgeMid);         % bd values at middle points of edges
        u1(fixedDof(idx)) = uD(:,1);
        u2(fixedDof(idx)) = uD(:,2);
        u = [u1; u2]; % Dirichlet bd condition is built into u
        f = f - A*u;  % bring affect of nonhomgenous Dirichlet bd condition to
        g = g - B*u;  % the right hand side
        g = g - mean(g);
        f(fixedDof) = u1(fixedDof);
        f(fixedDof+Nu) = u2(fixedDof);
    end
    % The case non-empty Dirichlet but g_D=[] corresponds to the zero Dirichlet
    % boundary condition and no modification is needed.

    % modfiy pressure dof for pure Dirichlet
    if isempty(Neumann)
        pDof = (1:Np-1)';
    end

    ufreeDof = [ufreeDof; Nu+ufreeDof];
    end % end of function getbdStokesP2P0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
% TODO: impose compatible condition int(pde.g_D*n)=0 numerically.
% NOTE: the data should be compatible, which is not easy. One
% special case is the enclose flow i.e. pde.g_D*n=0 everywhere.