% First stage is to match the calibration networks match (for cal-verify this will be disabled).
% Open Channel 1 data -- data collected with signal divider channel one connected to sum channel.  This is the master data set all other data will be corrected to this one. (other choice is to force all to zero phase)

% function MpdrChanMatchCal(Filename,Pathname)
% MpdrQuickLook(Filenane,Pathname) -- parse a data recording file for MPDR radar
%   filename -- filename of the file to parse

myRadar = mpdrRadar('.\Config\mpdr2PlusRadarConfig.yaml');

%

if exist('Filename',"var")
    if exist('Pathname',"var")
        myRadar.openDataRecordFile(Filename,Pathname);
    else
        myRadar.openDataRecordFile(Filename);
    end
else
    myRadar.openDataRecordFile();
end

% preallocate the cipData ...
cipData = zeros(myRadar.getNumChan(),...
    myRadar.getPriSize(),...
    myRadar.getNumPris(),...
    'like',1j);

[cipData,cipMetaData] = myRadar.getCIP(cipData);

%

%% Pulse Compress
% Pulse compress the CIPs
LrChans=radChan.getLrChans();
SrChans=radChan.getSrChans();
FigOrder=radChan.getFigOrder();

% create pulse compressors
LrPc = PulseCompressor(waveformTypes.LfmUpChirp,...
    cipMetaData.bandWidth(LrChans(1)),...
    cipMetaData.pulseLength_s(LrChans(1)),...
    cipMetaData.sampleRate_sps(uint32(radChan.sumSr)),...
    cipMetaData.priSize);
SrPc = PulseCompressor(waveformTypes.LfmUpChirp,...
    cipMetaData.bandWidth(SrChans(1)),...
    cipMetaData.pulseLength_s(SrChans(1)),...
    cipMetaData.sampleRate_sps(uint32(radChan.sumSr)),...
    cipMetaData.priSize);

pcData=zeros(size(cipData),'like',1j);

% pulse compress long range channels...
for ii=LrChans
    pcData(ii,:,:)=LrPc.compress(cipData(ii,:,:));
end
% myResult=LrPc.compress(conj(cipData(6,:,:)));

% pulse compress short range channels...
for ii=SrChans
    pcData(ii,:,:)=SrPc.compress(cipData(ii,:,:));
end

%


%% Plot PC results in dB
plotCube(pcData,cipMetaData,['Pulse compressed channels: ' cipMetaData.Filename],'MagDBPeak');

%
% get access to the peaks and examine the differences

% load MpdrQuickLook.mat;
LrIndex=uint32(radChan.sumLr);
SrIndex=uint32(radChan.sumSr);

pcDataBlob=reshape(pcData,cipMetaData.numChannels,[]);

pcDataSum=pcDataBlob([LrIndex SrIndex],:);

% normilize to max value...

maxValue = max(abs(pcDataSum),[],2);

% put it into log form:

pcDataSum_dB=(20*log10(abs(pcDataSum)./maxValue));

pcDataBlob_dB=pcDataBlob;
pcDataBlob_dB(LrChans,:)=(20*log10(abs(pcDataBlob(LrChans,:))./maxValue(1,1)));
pcDataBlob_dB(SrChans,:)=(20*log10(abs(pcDataBlob(SrChans,:))./maxValue(2,1)));

% find the peaks, Each peak  has to be > - 5 dB of the largest value,
% plus be a local maximum (greater than the cell before it and greater than the cell after it)

% P1 Signal is > - 5dB/peak value
p1=(pcDataSum_dB(:,2:end-1) > -5.0);
% p2 Signal is > prior cell
p2=(pcDataSum_dB(:,1:end-2)<pcDataSum_dB(:,2:end-1));
% p3 Signal is > next cell
p3=(pcDataSum_dB(:,2:end-1)<pcDataSum_dB(:,3:end));
% p4 is the logical and of P1-3 (aligning for index differences)
p4=p1&p2&(p3-1);
% find the non-zero cells
LrPeaks = find(p4(1,:))+1;
SrPeaks = find(p4(2,:))+1;

%
%% Plot the first few LR PRIs with the detections overlaid...

plotPris=3;
f1=figure;
set(f1,'position',[40 50 1500 600]);
set(f1,'Name',['Time Domain dB data: ' cipMetaData.Filename]);
colors=["b" "b" "r" "r" "g" "g" "c" "c"];
for ii=LrChans
    plot(pcDataBlob_dB(ii,1:(plotPris*cipMetaData.priSize)),colors(ii));
    hold on
    plot(LrPeaks(1,1:plotPris),pcDataBlob_dB(ii,LrPeaks(1,1:plotPris)),'ro');
    hold on
end
figure(f1);
hold on;
% plot(peaks(1,1:plotPris),pcDataBlob_dB(LrIndex,peaks(1,1:plotPris)),'ro');

%
%% Plot the first few SR PRIs with the detections overlaid...

f1=figure;
set(f1,'position',[40 50 1500 600]);
set(f1,'Name',['Time Domain dB data: ' cipMetaData.Filename]);
colors=["b" "b" "r" "r" "g" "g" "c" "c"];
for ii=SrChans
    plot(pcDataBlob_dB(ii,1:(plotPris*cipMetaData.priSize)),colors(ii));
    hold on
    plot(SrPeaks(1,1:plotPris),pcDataBlob_dB(ii,SrPeaks(1,1:plotPris)),'ro');
    hold on
end
figure(f1);
hold on;
% plot(peaks(1,1:plotPris),pcDataBlob_dB(LrIndex,peaks(1,1:plotPris)),'ro');

%
%% Convert to phase and amplitude...

lrPeakSamples = pcDataSum(1,LrPeaks);
srPeakSamples = pcDataSum(1,SrPeaks);
allPeaks = zeros(cipMetaData.numChannels,length(LrPeaks));
allPeaks(LrChans,:)= pcDataBlob(LrChans,LrPeaks);
allPeaks(SrChans,:)= pcDataBlob(SrChans,SrPeaks);
lrPeakMag=abs(lrPeakSamples);
srPeakMag=abs(srPeakSamples);

%% Caluclate the calibration corrections for each pulse...
% arbitrally choose ideal phase at zero degrees... sum channel (for visualization reasons).

idealSamples=zeros(cipMetaData.numChannels,length(lrPeakMag )); % preallocate
idealSamples(LrChans,:)=[lrPeakMag;lrPeakMag;lrPeakMag;lrPeakMag];
idealSamples(SrChans,:)=[srPeakMag;srPeakMag;srPeakMag;srPeakMag];

idealSamples=complex(idealSamples);


% TODO: decide if SR cals to LR or to SR...
% Currently SR cals to SR and LR to LR...
calCorrections(LrChans,:)=idealSamples(LrChans,:) ./ allPeaks(LrChans,:);
% calCorrections(SrChans,:)=idealSamples(LrChans,:) ./ allPeaks(SrChans,:);
calCorrections(SrChans,:)=idealSamples(SrChans,:) ./ allPeaks(SrChans,:);
calCorr = mean(calCorrections,2);
save('CalCorr.mat',"calCorr",'-mat');

%
%% Calibrate the data, convert to db and plot...

calPcDataBlob=pcDataBlob.*calCorr;

maxValue= zeros(cipMetaData.numChannels,1);
maxValue(LrChans,1)=max(abs(calPcDataBlob(LrIndex,:)),[],2);
maxValue(SrChans,1)=max(abs(calPcDataBlob(SrIndex,:)),[],2);

calPcDataBlob_db = zeros(size(calPcDataBlob));
calPcDataBlob_dB=(20*log10(abs(calPcDataBlob./maxValue)));

%
%% Plot the first few Corrected PRIs with the detections overlaid...

f2=figure;
set(f2,'position',[40 50 1500 600]);
set(f2,'Name',['Time Domain dB data: ' cipMetaData.Filename]);
colors=["b" "b" "r" "r" "g" "g" "c" "c"];
for ii=LrChans
    plot(calPcDataBlob_dB(ii,1:(plotPris*cipMetaData.priSize)),colors(ii));
    hold on
    
    plot(LrPeaks(1,1:plotPris),calPcDataBlob_dB(ii,LrPeaks(1,1:plotPris)),'ro');
    hold on
end
hold off;

%

f5=figure;
set(f5,'position',[40 50 1500 600]);
set(f5,'Name',['Time Domain dB data: ' cipMetaData.Filename]);
colors=['b' 'b' 'r' 'r' 'g-' 'g-' 'c' 'c'];
for ii=SrChans
    % for ii = [6,7,2,3] % put sum on top
    plot(calPcDataBlob_dB(ii,1:(plotPris*cipMetaData.priSize)),colors(ii));
    hold on
    
    %     plot(SrPeaks(1,1:plotPris),calPcDataBlob_dB(ii,SrPeaks(1,1:plotPris)),'ro');
    %     hold on
end
for ii=SrChans
    % for ii = [6,7,2,3] % put sum on top
    %     plot(calPcDataBlob_dB(ii,1:(plotPris*cipMetaData.priSize)),colors(ii));
    %     hold on
    
    plot(SrPeaks(1,1:plotPris),calPcDataBlob_dB(ii,SrPeaks(1,1:plotPris)),'ro');
    hold on
end
hold off;

%

peakSamples = zeros(8,length(LrPeaks));
peakSamples(LrChans,:)=pcDataBlob(LrChans,LrPeaks(1,:));
peakSamples(SrChans,:)=pcDataBlob(SrChans,SrPeaks(1,:));
[peakPhase_r,unCalPeakMag]=cart2pol(real(peakSamples),imag(peakSamples));
unCalPeakPhase_d = rad2deg(peakPhase_r);
peakSamples(LrChans,:)=calPcDataBlob(LrChans,LrPeaks(1,:));
peakSamples(SrChans,:)=calPcDataBlob(SrChans,SrPeaks(1,:));
[peakPhase_r,calPeakMag]=cart2pol(real(peakSamples),imag(peakSamples));
calPeakPhase_d = rad2deg(peakPhase_r);

%
%% LR Cal Verify

f3=figure;
colors=["b" "b" "r" "r" "g-" "g-" "k" "k"];
tiledlayout(2,2);
set(f3,'position',[40 50 1500 600]);
set(f3,'Name',['Uncalibrated vs Calibrated Peaks: ' cipMetaData.Filename]);
maxValue = max(max(unCalPeakMag));
rOrder=10^(floor(log10(maxValue)));
plotLimit=ceil(maxValue/rOrder)*rOrder;
nexttile;

for ii=1:8
    plot(unCalPeakMag(ii,:),colors(ii));
    title('Uncal mag');
    ylim([0 plotLimit]);
    hold on
end
nexttile;

for ii=1:8
    plot(unCalPeakPhase_d(ii,:),colors(ii));
    title('Uncal Phase');
    ylim([-180 180])
    hold on
end
nexttile;
for ii=1:8
    plot(calPeakMag(ii,:),colors(ii));
    title('Cal Mag');
    ylim([0 plotLimit]);
    hold on
end
nexttile;

for ii=1:8
    plot(calPeakPhase_d(ii,:),colors(ii));
    title('Cal Phase');
    ylim([-180 180]);
    hold on
end

%

MagAvg = mean(calPeakMag,2);
MagStd = std(calPeakMag(:,:),0,2);
PhaseAvg = mean(calPeakPhase_d,2);
PhaseStd = std(calPeakPhase_d,0,2);
for ii=FigOrder
    fprintf("File: %s: chan: %-12s  Mag: %-10.2g Sigma: %-10.2g Variance: %-7.4f Phase(d): %+-10.2f Phase Sigma: %-10.2f \n",...
        cipMetaData.Filename,radChan.getString(ii),MagAvg(ii,1),MagStd(ii,1),MagStd(ii,1)/MagAvg(ii,1),PhaseAvg(ii,1),PhaseStd(ii,1))
end

%
%% Plot the different channels on separate plots to see each independently....

plotCube(calPcDataBlob,cipMetaData,['Pulse compressed channels: ' cipMetaData.Filename],'MagDBPeak');

%

whos pcData
sumChan=radChan.sumLr;
auxChan=radChan.auxLr;
dazChan=radChan.dAzLr;
delChan=radChan.dElLr;
rZero=cipMetaData.cells.rangeZero(sumChan);
% iWin=cipMetaData.cells.PulseLength_samp(sumChan);
iWin=5;
iOffset=rZero-iWin-1;
figure, hold on
plot(20*log10(abs(pcData(sumChan,rZero-iWin:rZero+iWin,1))),'b')
plot(20*log10(abs(pcData(auxChan,rZero-iWin:rZero+iWin,1))),'r')
plot(20*log10(abs(pcData(dazChan,rZero-iWin:rZero+iWin,1))),'color',[0.3 0.4 0.6])
plot(20*log10(abs(pcData(delChan,rZero-iWin:rZero+iWin,1))),'m')
[sumLoc,sumMag]=peakfinder(20*log10(abs(pcData(sumChan,:,1))),[],100,1,false,true);
[auxLoc,auxMag]=peakfinder(20*log10(abs(pcData(auxChan,:,1))),[],100,1,false,true);
[dazLoc,dazMag]=peakfinder(20*log10(abs(pcData(dazChan,:,1))),[],100,1,false,true);
[delLoc,delMag]=peakfinder(20*log10(abs(pcData(delChan,:,1))),[],100,1,false,true);
plot(sumLoc-iOffset,sumMag,'bv',auxLoc-iOffset,auxMag,'rv');
plot(dazLoc-iOffset,dazMag,'color',[0.3 0.4 0.6],'LineStyle',"none","Marker","v");
plot(delLoc-iOffset,delMag,'mv');
hold off

%

% hold off
% ds = 100;  % Downsample factor
%   dt = .001; % Time step
%   ds_dt = ds*dt; % Time delta after downsampling
%   t0 = 1;
%   t = t0:dt:5 + t0;
%   x = 0.2-sin(0.01*2*pi*t)+3*cos(7/13*2*pi*t+.1)-2*cos((1+pi/10)*2*pi*t+0.2)-0.2*t;
%   x(end) = min(x);
%   x_ds = x(1:ds:end); % Downsample to test interpolation
%   [minLoc, minMag] = peakfinder(x_ds, .8, 0, 1, false, true);
%   minT = t0 + (minLoc - 1) * ds_dt; % Take into account 1 based indexing
%   p = plot(t,x,'-',t(1:ds:end),x_ds,'o',minT,minMag,'rv');
%   set(p(2:end), 'linewidth', 2); % Show the markers more clearly
%   legend('Actual Data', 'Input Data', 'Estimated Peaks');

%

pcData=reshape(pcData,8,[]);
[sumLoc,sumMag]=peakfinder(20*log10(abs(pcData(sumChan,:,1))),[],100,1,false,true);
[auxLoc,auxMag]=peakfinder(20*log10(abs(pcData(auxChan,:,1))),[],100,1,false,true);
[dazLoc,dazMag]=peakfinder(20*log10(abs(pcData(dazChan,:,1))),[],100,1,false,true);


[delLoc,delMag]=peakfinder(20*log10(abs(pcData(delChan,:,1))),[],100,1,false,true);
fprintf("Sum:%-7.2f Aux:%-7.2f Delta:%-7.2f\n",sumLoc,auxLoc,sumLoc-auxLoc);
fprintf("Sum:%-7.2f Del:%-7.2f Delta:%-7.2f\n",sumLoc,delLoc,sumLoc-delLoc);
fprintf("Sum:%-7.2f Daz:%-7.2f Delta:%-7.2f\n",sumLoc,dazLoc,sumLoc-dazLoc);