%function job = RangeDopplerRadar_MAIN(s)
%%% LC_og.m
close all; clear all; clc;

%%
IsDumpFig = 1;

%%
%T = 1.2;      % Duration
T = 1;      % Duration
B = 1;  % Bandwidth
n = 25; % range-Dopper radar, N = n*n;
m = 80; %m = 150;
q = 2;      % <==

s = 10;


%%
t = sort( (rand(m,1)-0.5)*T );
sgm = norm(t,2)/sqrt(m);


k = -n/2:n/2-1;
l = -n/2:n/2-1;
p = -(n+1)*n/2:(n-1)*n/2-1;
beta1 = B/T;
beta0 = 5;

Delta_tau = q/B;
Delta_alpha = n*q/T;

f_LC = @(t) exp(2*pi*1i*(beta1*t/2+beta0).*t);

%% Construct the received signal
switch 0
    case 1 % random targets
        S_xi = randsample(n,s,'false');
        S_eta = randsample(n,s,'false'); 
        S = sort( S_xi + n*(S_eta-1) , 'ascend');
        theta = zeros(n*n,1); theta(S) = 1 + 0.5*(randn(s,1) + 1i*randn(s,1))/sqrt(2);
        xi = zeros(n,1); xi(S_xi) = 0.4 * (rand(s,1)*2-1) * (Delta_alpha/n)/q; % <===
        eta = zeros(n,1);  eta(S_eta) = 0.4 * (rand(s,1)*2-1) * (Delta_alpha/n)/q ; % <=== since (Delta_alpha/n) is the grid size
    case 2 % two subwavelength time delay
        %s=2;
        %S = [n/2;n/2+1]; 
        %sigma = zeros(n,1); sigma(S) = 1+0.1*randn(s,1); 
        %xi = zeros(n,1); xi(S) = [0.8;-0.8] * Delta_tau/q; % <===
    case 0
        %load RangeDopplerRadar_configdata
        load RangeDopplerRadar_configdata_keep03
end
save RangeDopplerRadar_configdata



%%      
%xi(S_xi) = 0.01; eta(S_eta) = 0.01;


tau = (k')*Delta_tau;
alpha = (l')*Delta_alpha;
gamma = (p')*Delta_tau; % = beta_1 tau_k + alpha_l
tautil = tau + xi;
alphatil = alpha + eta;

gammatil = repmat(tautil,n,1) + kron(alphatil,ones(n,1))/beta1;
zeta = repmat(xi,n,1) + kron(eta,ones(n,1))/beta1;

Y = exp( -2i*pi*B*kron(t,transpose(gammatil)) ) * ( repmat(f_LC(-tautil),n,1).*theta );

[tautil(1:10), alphatil(1:10)]

F = exp(-2*pi*1i*q*kron(t,p));
G = diag(t)*exp(-2*pi*1i*q*kron(t,p))  /sgm;
%X1 = theta .* repmat( f_LC(-tau) .* f_LC(-xi) .*exp(-2i*pi*q *(k'.*xi)/T), n,1);
%X2 = 2i*pi*B* (zeta.*X1);


X = zeros(n^2,1); Xp = zeros(n^2,1);
%X(S) = f_LC(-tau(S)).*f_LC(-xi(S)).* sigma(S).*exp(2*pi*1i*q*(k(S)').*xi(S)/T);
X = repmat(  f_LC(-tau).*f_LC(-xi).* exp(2*pi*1i*q*(k').*xi/T), n, 1  )  .*theta;
Xp = (-2*pi*1i*(q/Delta_tau)*zeta).*X  * sgm;

Yapprox_fix0 = F*X ;
Yapprox_fix1 = F*X + G*Xp;
% 
%[Y,Yapprox_fix0, Yapprox_fix1]
% 
GridingError_fix0 = norm(Y-Yapprox_fix0,2) / norm(Y,2);
GridingError_fix1 = norm(Y-Yapprox_fix1,2) / norm(Y,2);



%% Add noise
SNR = 100; % <=======
E = (1/SNR) * (norm(Y)/sqrt(m)) * (randn(m,1) + 1i*randn(m,1))/sqrt(2);
%E = zeros(m,1);


%%
fc_r = 10;
fceps = 0.401*Delta_tau;
%
fc_p = -fc_r*(n^2)/2:fc_r*(n^2)/2-1;
fc_Delta_tau = Delta_tau/fc_r;
fc_gamma = (fc_p')*fc_Delta_tau;
fc_abs_theta = filterconv(gammatil(S), abs(theta(S)), fc_gamma, fceps);


%% Reconstruction without perturbation correcting, OMP
if(0)
    X_til = OMP(F,Y+E, s);
    theta_til =  X_til;
    
    %FilteredError = norm(X - X_til(1:n),2)/norm(X,2);
    fc_abs_theta_til = filterconv(gamma, abs(theta_til), fc_gamma, fceps);
    %FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/sqrt(length(fc_abs_theta));
    FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/norm(fc_abs_theta);
    
    if(IsDumpFig)
        RangeDopplerRadar_plot(100, 'Range-Doppler Image, off-grid fixing off, OMP.', 'fig_RangeDopplerRadar_fixpt_gridfixOFF_OMP', ...
                             m, n, s, SNR, coherence(F), GridingError_fix0, FilteredError, ...
                             gammatil(S), abs(theta(S)), gamma, abs(theta_til), ...
                             tau, alpha, abs(theta), tautil(S_xi), alphatil(S_eta) )
    end
    
        
    %[temp, S_til] = sort(abs_theta_til, 'descend'); S_til = sort(S_til(1:s),'ascend');
    %job = ( sum(S == S_til) == s );       %job = (FilteredError < 0.4 );
    
    clear x_til theta_til
end
%% Reconstruction without perturbation correcting, BP
if(0)
    X_til = BP_YALL1(F,Y+E);
    %X_til = LASSO(F,Y+E, 10/SNR);
    theta_til = X_til;
    
    %FilteredError = norm(X - X_til(1:n),2)/norm(X,2);
    fc_abs_theta_til = filterconv(gamma, abs(theta_til), fc_gamma, fceps);
    %FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/sqrt(length(fc_abs_theta));   
    FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/norm(fc_abs_theta);   

    if(IsDumpFig)
        RangeDopplerRadar_plot(200, 'Range-Doppler Image, off-grid fixing off, BP.', 'fig_RangeDopplerRadar_fixpt_gridfixOFF_BP', ...
                             m, n, s, SNR, coherence(F), GridingError_fix0, FilteredError, ...
                             gammatil(S), abs(theta(S)), gamma, abs(theta_til), ...
                             tau, alpha, abs(theta), tautil(S_xi), alphatil(S_eta) )
                         
    end
    
    clear x_til theta_til
end
%% Reconstruction with perturbation correcting, BP
if(0)
    X_til = BP_YALL1([F,G],Y+E); 
    %X_til = LASSO(A,Y+E, 10/SNR);

    [temp, S_til] = sort(abs(X_til(1:n^2)),'descend');
    temp = -( X_til(n^2+1:2*n^2)./X_til(1:n^2) )/(2i*pi*B);  temp( abs(X_til(1:n^2))<1e-8 )=0.0;
    zeta_til = zeros(n^2,1); zeta_til(S_til) = real(temp(S_til));
    theta_til = X_til(1:n^2); 
    
    fc_abs_theta_til = filterconv(gamma+zeta_til, abs(theta_til), fc_gamma, fceps);
    FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/norm(fc_abs_theta);

    if(IsDumpFig)
        RangeDopplerRadar_plot(300, 'Range-Doppler Image, off-grid fixing on, BP.', 'fig_RangeDopplerRadar_fixpt_gridfixON_BP', ...
                             m, n, s, SNR, coherence([F,G]), GridingError_fix1, FilteredError, ...
                             gammatil(S), abs(theta(S)), gamma+zeta_til, abs(theta_til), ...
                             tau, alpha, abs(theta), tautil(S_xi), alphatil(S_eta) )
    end
 
    clear x_til theta_til S_til zeta_til 
end
%% Reconstruction with perturbation correcting BPLOT
if(0)
    band = 1;
    [X_til, SSS] = BP_LOT_FG( BP_YALL1([F G],Y+E), F, G, Y+E, s, band); 

    [temp, S_til] = sort(abs(X_til(1:n^2)),'descend');
    temp = -( X_til(n^2+1:2*n^2)./X_til(1:n^2) )/(2i*pi*B);  temp( abs(X_til(1:n^2))<1e-8 )=0.0;
    zeta_til = zeros(n^2,1); zeta_til(S_til) = real(temp(S_til));
    theta_til = X_til(1:n^2);
    
    fc_abs_theta_til = filterconv(gamma+zeta_til, abs(theta_til), fc_gamma, fceps);
    FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/norm(fc_abs_theta);

    if(IsDumpFig)
        RangeDopplerRadar_plot(400, 'Range-Doppler Image, off-grid fixing on, BPLOT.', 'fig_RangeDopplerRadar_fixpt_gridfixON_BPLOT', ...
                             m, n, s, SNR, coherence([F,G]), GridingError_fix1, FilteredError, ...
                             gammatil(S), abs(theta(S)), gamma+zeta_til, abs(theta_til), ...
                             tau, alpha, abs(theta), tautil(S_xi), alphatil(S_eta) )
    end
    
    clear x_til theta_til S_til zeta_til 
end


%% Reconstruction with SCOMP
if(1)
    band = 1;
    X_til = SCOMP(F,G,Y+E,s,band); 

    [temp, S_til] = sort(abs(X_til(1:n^2)),'descend');
    temp = -( X_til(n^2+1:2*n^2)./X_til(1:n^2) )/(2i*pi*B);  temp( abs(X_til(1:n^2))<1e-8 )=0.0;
    zeta_til = zeros(n^2,1); zeta_til(S_til) = real(temp(S_til));
    theta_til = X_til(1:n^2) ; 

    fc_abs_theta_til = filterconv(gamma+zeta_til, abs(theta_til), fc_gamma, fceps);
    FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/norm(fc_abs_theta);

    if(IsDumpFig)
        RangeDopplerRadar_plot(500, 'Range-Doppler Image, off-grid fixing on, SCOMP.', 'fig_RangeDopplerRadar_fixpt_gridfixON_SCOMP', ...
                             m, n, s, SNR, coherence([F,G]), GridingError_fix1, FilteredError,  ...
                             gammatil(S), abs(theta(S)), gamma+zeta_til, abs(theta_til), ...
                             tau, alpha, abs(theta), tautil(S_xi), alphatil(S_eta) )
    end
                    
    %figure(111); plot(fc_gamma/fc_Delta_tau, fc_abs_theta, 'r', fc_gamma/fc_Delta_tau, fc_abs_theta_til, 'b'); 
    %xlim([min(fc_gamma)/fc_Delta_tau, max(fc_gamma)/fc_Delta_tau]); ylim([0,2]);
    
    SCOMP_X_til = X_til; SCOMP_theta_til = theta_til; SCOMP_zeta_til = zeta_til;
    clear x_til theta_til S_til zeta_til 
end

%% Reconstruction with NLS with initial guess from SCOMP
if(1)
    
    [tautil(1:10), alphatil(1:10)]
    
    %// Choose initial guess 
    X_til_init = SCOMP_X_til; theta_til_init = SCOMP_theta_til; zeta_til_init = SCOMP_zeta_til;
    S_til_init = sort( find(theta_til_init ~= 0), 'ascend' );
    
    %zeta = repmat(xi,n,1) + kron(eta,ones(n,1))/beta1;
    tautil_init = tautil; %zeros(n,1); %zeta_til(1:n);
    alphatil_init = alphatil; %zeros(n,1);
    

    %// NLS
    %lb = [-ones(2*n,1); -0.5*Delta_tau*ones(n,1)];
    %rb = [ones(2*n,1); 0.5*Delta_tau*ones(n,1)];
    [XX,resnorm,residual,exitflag] ...
        = lsqnonlin(@(X)RangeDopplerRadar_sigfit(X, n, Y+E, S_til_init, t, B, beta1, f_LC), ...
                    [real(theta_til_init); imag(theta_til_init); tautil_init; alphatil_init] );
    %resnorm
    %residual
    exitflag
    
    nn = n*n;
    QQ = XX(2*nn+1:end);
    xi_til = QQ(1:n)-tau;
    eta_til = QQ(n+1:2*n)-alpha;
    theta_til = XX(1:nn) + 1i*XX(nn+1:2*nn);
    zeta_til = repmat(xi_til, n, 1) + kron(eta_til, ones(n,1))/beta1;
    
    fc_abs_theta_til = filterconv(gamma+zeta_til, abs(theta_til), fc_gamma, fceps);
    FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/norm(fc_abs_theta);

    if(IsDumpFig)
        RangeDopplerRadar_plot(600, 'Range-Doppler Image, NLS wuth initial guess from SCOMP.', 'fig_RangeDopplerRadar_fixpt_NLS', ...
                             m, n, s, SNR, coherence([F,G]), GridingError_fix1, FilteredError,  ...
                             gammatil(S), abs(theta(S)), gamma+zeta_til, abs(theta_til), ...
                             tau, alpha, abs(theta), tautil(S_xi), alphatil(S_eta) )
    end
                    
    %figure(111); plot(fc_gamma/fc_Delta_tau, fc_abs_theta, 'r', fc_gamma/fc_Delta_tau, fc_abs_theta_til, 'b'); 
    %xlim([min(fc_gamma)/fc_Delta_tau, max(fc_gamma)/fc_Delta_tau]); ylim([0,2]);
    
    clear x_til theta_til S_til zeta_til 
end

%% Reconstruction with NLS with initial guess from SCOMP, ver2
if(0)
    
    %// Choose initial guess 
    X_til_init = SCOMP_X_til; theta_til_init = SCOMP_theta_til; zeta_til_init = SCOMP_zeta_til;
    S_til_init = sort( find(theta_til_init ~= 0), 'ascend' );
    
    %zeta = repmat(xi,n,1) + kron(eta,ones(n,1))/beta1;
    tautil_init = zeros(n,1); %zeta_til(1:n);
    alphatil_init = zeros(n,1);

    nn = n*n;
    %// NLS
    %lb = [-ones(2*n,1); -0.5*Delta_tau*ones(n,1)];
    %rb = [ones(2*n,1); 0.5*Delta_tau*ones(n,1)];
    [XX,resnorm,residual,exitflag] ...
        = lsqnonlin(@(X)RangeDopplerRadar_sigfit(X, n, Y+E, (1:nn), t, B, beta1, f_LC), ...
                    [real(theta_til_init); imag(theta_til_init); tautil_init; alphatil_init] );
    %resnorm
    %residual
    exitflag
    
    
    QQ = XX(2*nn+1:end);
    theta_til = XX(1:nn) + 1i*XX(nn+1:2*nn);
    [temp, SS] = sort( abs(theta_til), 'descend'); theta_til(setdiff((1:nn),SS(1:s)))=0;   
    
    zeta_til = repmat(QQ(1:n), n, 1) + kron(QQ(n+1:2*n), ones(n,1))/beta1;
    
    fc_abs_theta_til = filterconv(gamma+zeta_til, abs(theta_til), fc_gamma, fceps);
    FilteredError = norm(fc_abs_theta_til - fc_abs_theta)/norm(fc_abs_theta);

    if(IsDumpFig)
        RangeDopplerRadar_plot(700, 'Range-Doppler Image, NLS wuth initial guess from SCOMP.', 'fig_RangeDopplerRadar_fixpt_NLSv2', ...
                             m, n, s, SNR, coherence([F,G]), GridingError_fix1, FilteredError,  ...
                             gammatil(S), abs(theta(S)), gamma+zeta_til, abs(theta_til), ...
                             tau, alpha, abs(theta), tautil(S_xi), alphatil(S_eta) )
    end
                    
    %figure(111); plot(fc_gamma/fc_Delta_tau, fc_abs_theta, 'r', fc_gamma/fc_Delta_tau, fc_abs_theta_til, 'b'); 
    %xlim([min(fc_gamma)/fc_Delta_tau, max(fc_gamma)/fc_Delta_tau]); ylim([0,2]);
    
    clear x_til theta_til S_til zeta_til 
end

%%