% Convert pulse-compressed IQ data to radar moments
% This contains most of the previous contents of the demo file
%
% INPUTS:
%
% iq_h - one NCIP - ngate range bins x NCip CPIs x Np PRIs  to get radar moments (Z, Vr and
% sigma_v) per range bin. The Ncip CPIS are incoherently integrated
%
% radar - Parameters of the radar system
%      delr_m - Range resolution; separates any two range gates
%    noise_dB - Estimates of noise in the measurements.
%     calZ_dB - Calibration factors for reflectivity in/out of fill pulse
%      prf_hz - Pulse repetition frequency
%    lambda_m - Carrier wavelength
%   censor_dB - Threshold for SNR censoring
%        date - String representation of date. Only used for plotting
%        time - String representation of time. Only used for plotting
%     fillpad - Number of gates "padding" the pulse fill region
%    fillgate - Number of gates during which the pulse fill is actually
%               being transmitted
%
% OUTPUTS:
% moments - fields of interpreted radar data - one value per range bin
%           Z - Reflectivity
%           V - Radial velocity
%           W - Spectrum width
%           SQI - Signal Quality Index
%
% xx and yy - Cartesian coordinate grids for measurement fields

function moments_out = getRadarMomentsNcip(radar,  iq_h, censor) %, singlePol, singlePol, range_compensate, sr_lr_calcoords,, filtClutter

% Enable/disable debug code
%debug_flag = true; % Currently not used

% Configure range gate decimation factor (1 for no downsampling)
gstep = 1;

% Get data dimensions
[ngate_before_decimation, npri, Ncip] = size(iq_h);
ngate = numel(iq_h(1:gstep:end, 1)); % number of gates after decimation

% fprintf('Number Pulses: %d\n',nradial);
% fprintf('Number Range Cells: %d -> %d\n',ngate_before_decimation,ngate)
% fprintf('Decimation Factor: %1d\n',gstep);
%               

%% Filtering

% Clutter filter coefficients
% filtB = [0.7106   -3.5427    7.0750   -7.0750    3.5427   -0.7106];
% filtA = [1.0000   -4.3170    7.4908   -6.5180    2.8380   -0.4930];

if exist('filtClutter','var') && filtClutter
    % dim = 2 is angular dimension
    filtIq_h = filter(filtB, filtA, iq_h, [], 2);
end

t0offset = 0; 

% % Matrices have some unused slices for alternate estimation methods
% Rh = zeros(ngate, nray, 2, 'single');  % lags 0 and 1
% % Sh_pp_temp = zeros(ngate, nray, 'single');  % pulse pair power
Sh_sp_temp = zeros(ngate-t0offset,'single');  % spectral processing power
nci_mag_sq_axis = zeros(ngate-t0offset,npri,'single'); 
nci_mag_sq_shift =  zeros(ngate-t0offset,npri,'single');
Sh_pp_temp_sum = zeros(ngate-t0offset,1);
Sh_pp_temp = zeros(ngate-t0offset,1);


% Unambiguous Doppler velocity
va_temp = radar.lambda_m * 0.25 * radar.prf_hz;
%fprintf('Unambiguous Doppler Velocity (m/s): %0.2f\n',va_temp);

% Do we want to use fading memory here as well? MPS.
for cipidx = 1:Ncip
    Xh = iq_h(t0offset+1:gstep:end,:,cipidx); %#ok<*PFBNS>
    tRh(:, 1) = matxcov(Xh,0);   % lag-0
    tRh(:, 2) = matxcov(Xh, 1);   % lag-1
    Sh_pp_temp = (real(tRh(:,1)));
    Sh_pp_temp_sum = Sh_pp_temp_sum + Sh_pp_temp;
    % Sh_sp
    Np = size(Xh,2);
    mag_sq_axis2 = abs(fft(Xh,[],2)).^2./Np;
    nci_mag_sq_axis = nci_mag_sq_axis + mag_sq_axis2;
    mag_sq_shift2 = fftshift(mag_sq_axis2,2);
    nci_mag_sq_shift = nci_mag_sq_shift + mag_sq_shift2;
end

nci_mag_sq_axis = nci_mag_sq_axis/Ncip;
nci_mag_sq_shift = nci_mag_sq_shift/Ncip;

power2 = sum(nci_mag_sq_axis,2)./Np;
Sh_sp_temp = power2;

% Vr_pp & Vr_sp
x_axis = [ 0:1:Np-1]*va_temp*2/(Np-1);
vel_axis = x_axis - max(x_axis)/2; % Change from 0 to max -> -max/2 to max/2
% mag_sq_shift = fftshift(abs(fft(Xh,[],2)).^2./Np);


mag_sq_norm2 = nci_mag_sq_shift./(sum(nci_mag_sq_shift,2));
vr_est2 = sum(vel_axis.*mag_sq_norm2,2);
%vr_est2 = nci_mag_sq_shift;
%***TEMP***
%plot(vel_axis,mag_sq_norm2);shg
% keyboard
[aa bb] = max(mag_sq_norm2,[],2);
%     vr_est2 = vel_axis(bb);
%     vr_sp_temp(:,iaz) = vr_est2; %***check***
vr_sp_temp = vel_axis(bb)';

w_est2 = sum(((vel_axis-vr_est2).^2.*mag_sq_norm2),2).^0.5;
sw_sp_temp = w_est2;



%% Other required parameters

% Power estimates from lag-0
Sh_sp = Sh_sp_temp ; %- 10^(0.1 * radar.noise_dB);

% Temporary use of original Sh estimate for SNR censoring
SNRh_dB = 10*log10(Sh_sp) - radar.noise_dB(1);

Sh_sp(Sh_sp < 0) = 0;

% Range gates
rr = 1e-3 * (0.5:gstep:size(iq_h, 1)) * radar.delr_m;
rr = rr(:);
bins.r_km = rr;

bins.az_deg = 2;
bins.el_deg = 10;
%fprintf('Range Resolution (m): %0.2f\n',radar.delr_m);

% Unambiguous Doppler velocity
va = radar.lambda_m * 0.25 * radar.prf_hz;
%fprintf('Unambiguous Doppler Velocity (m/s): %0.2f\n',va);

% Per-bin calibration factors
% Changes at end of fill pulse
n1 = numel(1:gstep:radar.fillgate+radar.fillpad);
n2 = ngate - n1;
% zcal = [radar.calZ_dB(1)*ones(n1, 1); radar.calZ_dB(2)*ones(n2, 1)];

% % Cartesian coordinate grids with an extra azimuth row to close pcolor
% % These will be used for plotting and wind shear estimation
% [gaz,gr] = meshgrid(bins.az_deg, bins.r_km);
% gel = repmat(bins.el_deg,[ngate 1]);
% xx = gr .* cosd(gaz) .* cosd(gel);
% yy = gr .* sind(gaz) .* cosd(gel);

%% Moments
% Results in a structure with fields for each moment of interest
% Each field will have the full PPI as needed for plotting
% The field names (D for Z_DR, etc.) are conventional

% Reflectivity Z
% if (range_compensate)
%Z_uncal_sp = Sh_sp .* bins.r_km.^2;
% else
%     Z_uncal = Sh;
%     Z_uncal_pp = Sh_pp;
Z_uncal_sp = Sh_sp;
% end

% if (sr_lr_cal)
if radar.lr
    caldb = radar.calZ_dB_LR;
else
    caldb = radar.calZ_dB_SR;
end

moments.Z_sp = 10*log10(Z_uncal_sp) + caldb;
% else
% 
%moments.Z_sp = 10*log10(Z_uncal_sp);
%     moments.Z_pp = 10*log10(Z_uncal_pp);
%     moments.Z_sp = 10*log10(Z_uncal_sp);
% end

% just Z in terms of range.
% plot( moments.Z_sp);
% pause(0.25);
% Mean velocity v_r
moments.V_sp = -1.*vr_sp_temp;
% Spectrum width sigma_v
moments.W_sp = sw_sp_temp;

% Signal Quality Index
% moments.SQI = abs(Rh(:,:,2)).^2 ./ abs(Rh(:,:,1)).^2;

%% Censoring
%censor = 0;
%xlim([1 433]);
% figure(66);
%  plot(1:433,moments.V_sp);
%  hold;
%  %plot(1:433,moments.W_sp);
%  %plot(1:433,moments.Z_sp);
% % 
% % %legend('V','W','Z');
% % legend('V','Z');
% % hold off;
%  pause(0.25);
% censor = false;
if censor
    % Mask out if horizontal polarization SNR is less than the threshold
    cmask = SNRh_dB < radar.censor_dB;

    % Convert censored radar moments, wind, and rainfal to NaN so they get plotted
    % as background color
    fields = fieldnames(moments);
    for f = 1:numel(fields)
        moment = moments.(fields{f});
        moment(cmask) = NaN;
        moments.(fields{f}) = moment;
    end
end
%plot(1:433,moments.V_sp);
%pause(0.25);
moments_out(:,1) = moments.Z_sp;
moments_out(:,2) = moments.V_sp;
moments_out(:,3) = moments.W_sp;
%keyboard % ***DEBUG

%keyboard % ***DEBUG
