ShowPlots=4;
spec_win = 1024;  % Window for Welch spectral estimation
addpath ../../model

%% Check GMSK output
if exist('gmsk_dout')
    load results/gmsk_data_comp.mat;
    if ShowPlots>=4
        figure; plot(real(gmsk_data_comp(1:size(gmsk_dout,1),1)));
        hold on ; plot(gmsk_dout(:,1),'r:'); hold off;
    end
    if real(gmsk_data_comp(1:size(gmsk_dout,1),1)) == gmsk_dout(:,1)
        display('Matched GMSK data to GMSK output!')
    else
        error('Output from GMSK is different than expected!')
    end
end

%% Check CFIR input
if exist('cfir_din') && exist('gmsk_dout')
    if ShowPlots>=4
        figure; plot(gmsk_dout(1:end-1,1));
        hold on ; plot(cfir_din(1:size(gmsk_dout,1)-1,1),'r:'); hold off;
    end
    %cfir_diff = gmsk_dout(1:end-1,1) - cfir_din(1:size(gmsk_dout,1)-1,1);
    if gmsk_dout(1:end-1,1) == cfir_din(1:size(gmsk_dout,1)-1,1)
        display('Matched GMSK output to CFIR data in!')
        clear gmsk_data_comp gmsk_dout cfir_diff;
    else
        error('Input to CFIR is different than expected!')
    end
end

%% Analyze CFIR sample output
if exist('cfir_dout')
    load results/yc_duc.mat;
    %cfir_start = min(find(cfir_vout>0));
    cfir_start = 40;   % From analyzing cfir_vout
    channels = 8;      % Number of channels to split

    %% Chop preceding zeroes and tail
    cfir_temp = cfir_dout(cfir_start:end-2);

    %% Split CFIR dout into channels and analyze
    cfir_chan = zeros(ceil(length(cfir_temp)/channels),channels);
    cfir_temp = [ cfir_temp ; zeros(numel(cfir_chan)-length(cfir_temp),1) ];
    for ii=1:channels
        cfir_chan(:,ii) = cfir_temp(ii:channels:end);
    end
    clear cfir_temp;
    cfir_chan = cfir_chan(1:end-1,:);   % Drop end row, may hold filler zeros
    %figure; plot(real(yc_duc(1:100,1)));
    %hold on ; plot(real(yc_quant(1:100,1)),'r:'); hold off;
    %figure; plot(real(yc_duc(1:100,1)));
    %hold on ; plot(cfir_chan(1:100,1),'r:'); hold off;
    if ShowPlots>=4
        for ii=1:channels
            if mod(ii,2)==1
                figure; plot(real(yc_duc(1:size(cfir_chan(1:end,ii),1),ceil(ii/2))));
            else
                figure; plot(imag(yc_duc(1:size(cfir_chan(1:end,ii),1),ceil(ii/2))));
            end
            hold on; plot(cfir_chan(1:end,ii),'r:'); hold off;
        end
    end

    %% Check samples match
    failed=0;
    for ch=1:4
        i_mismatches = 0; q_mismatches = 0;
        display(sprintf('Checking Carrier %d',ch));
        %for ii=1:size(cfir_temp,1)
        for ii=1:size(cfir_chan(1:end,1),1)
            if real(yc_duc(ii,ch)) ~= cfir_chan(ii,2*ch-1)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('I value mismatch on Channel %d at position %d',ch,ii));
                end
                i_mismatches = i_mismatches+1;
            end
            if imag(yc_duc(ii,ch)) ~= cfir_chan(ii,2*ch)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('Q value mismatch on Channel %d at position %d',ch,ii));
                end
                q_mismatches = q_mismatches+1;
            end
        end
        display(sprintf('Carrier %d: I mismatches = %d',ch,i_mismatches));
        display(sprintf('Carrier %d: Q mismatches = %d',ch,q_mismatches));
        display(sprintf('Carrier %d: Total mismatches = %d',ch,i_mismatches+q_mismatches));
        if (i_mismatches+q_mismatches)>0
            failed=1;
        end
    end
    if failed==1
        error('Output from CFIR does not match Matlab model CFIR output!')
    else
        display(sprintf('Matched CFIR output to Matlab model CFIR output! [Samples=%d]',size(cfir_chan,1)))
        clear cfir_dout cfir_chan yc_duc;
    end
end
%% Clear variables
clear failed ch i_mismatches q_mismatches;

%% Analyze Scaler sample output
if exist('scaler_dout')
    load results/scaled_yc_duc.mat;
    %scaler_start = min(find(scaler_vout>0));
    scaler_start = 45;   % From analyzing scaler_vout
    channels = 8;        % Number of channels to split

    %% Chop preceding zeroes and tail
    scaler_temp = scaler_dout(scaler_start:end-2);

    %% Split Scaler dout into channels and analyze
    scaler_chan = zeros(ceil(length(scaler_temp)/channels),channels);
    scaler_temp = [ scaler_temp ; zeros(numel(scaler_chan)-length(scaler_temp),1) ];
    for i=1:channels
        scaler_chan(:,i) = scaler_temp(i:channels:end);
    end
    clear scaler_temp;
    scaler_chan = scaler_chan(1:end-1,:);   % Drop end row, may hold filler zeros
    if ShowPlots>=4
        for i=1:channels
            if mod(i,2)==1
                figure; plot(real(scaled_yc_duc(1:size(scaler_chan(1:end,i),1),ceil(i/2))));
            else
                figure; plot(imag(scaled_yc_duc(1:size(scaler_chan(1:end,i),1),ceil(i/2))));
            end
            hold on; plot(scaler_chan(1:end,i),'r:'); hold off;
        end
    end

    %% Check samples match
    for ch=1:4
        failed=0;
        i_mismatches = 0; q_mismatches = 0;
        display(sprintf('Checking Scaler Carrier %d',ch));
        %for i=1:size(scaler_temp,1)
        for i=1:size(scaler_chan(1:end,1),1)
            if real(scaled_yc_duc(i,ch)) ~= scaler_chan(i,2*ch-1)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('I value mismatch on Channel %d at position %d',ch,i));
                end
                i_mismatches = i_mismatches+1;
            end
            if imag(scaled_yc_duc(i,ch)) ~= scaler_chan(i,2*ch)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('Q value mismatch on Channel %d at position %d',ch,i));
                end
                q_mismatches = q_mismatches+1;
            end
        end
        display(sprintf('Carrier %d: I mismatches = %d',ch,i_mismatches));
        display(sprintf('Carrier %d: Q mismatches = %d',ch,q_mismatches));
        display(sprintf('Carrier %d: Total mismatches = %d',ch,i_mismatches+q_mismatches));
        if (i_mismatches+q_mismatches)>0
            failed=1;
        end
    end
    if failed==1
        error('Output from scaler does not match Matlab model scaler output!')
    else
        display(sprintf('Matched scaler output to Matlab model scaler output! [Samples=%d]',size(scaler_chan,1)));
        clear scaler_dout scaler_chan failed;
    end
end
%% Clear variables
clear failed ch i_mismatches q_mismatches;

%% Analyze CIC input buffer output
if exist('cic_i_ipbuff_dout')
    %cic_ipbuff_start = min(find(cic_ipbuff_vout>0));
    cic_ipbuff_start = 419;   % From analyzing cic_ipbuff_vout

    %% Chop preceding zeroes and tail
    cic_i_ipbuff_temp = cic_i_ipbuff_dout(cic_ipbuff_start:end-2);
    cic_q_ipbuff_temp = cic_q_ipbuff_dout(cic_ipbuff_start:end-2);

    %% Adjust for rate change for CIC input and clock oversampling
    cic_i_ipbuff_temp2 = [];
    cic_q_ipbuff_temp2 = [];
    for ii=1:m_cic*clk_os:size(cic_i_ipbuff_temp)-n_carr
        %display(sprintf('Index %d',ii));
        cic_i_ipbuff_temp2 = [ cic_i_ipbuff_temp2; cic_i_ipbuff_temp(ii:ii+n_carr-1) ];
        cic_q_ipbuff_temp2 = [ cic_q_ipbuff_temp2; cic_q_ipbuff_temp(ii:ii+n_carr-1) ];
    end
    clear cic_i_ipbuff_temp cic_q_ipbuff_temp;

    %% Split cic_ipbuff_dout into channels and analyze
    cic_i_ipbuff_chan = zeros(ceil(length(cic_i_ipbuff_temp2)/n_carr),n_carr);
    cic_q_ipbuff_chan = zeros(ceil(length(cic_q_ipbuff_temp2)/n_carr),n_carr);
    cic_i_ipbuff_temp2 = [ cic_i_ipbuff_temp2 ; zeros(numel(cic_i_ipbuff_chan)-length(cic_i_ipbuff_temp2),1) ];
    cic_q_ipbuff_temp2 = [ cic_q_ipbuff_temp2 ; zeros(numel(cic_q_ipbuff_chan)-length(cic_q_ipbuff_temp2),1) ];
    for ii=1:n_carr
        cic_i_ipbuff_chan(:,ii) = cic_i_ipbuff_temp2(ii:n_carr:end);
        cic_q_ipbuff_chan(:,ii) = cic_q_ipbuff_temp2(ii:n_carr:end);
    end
    clear cic_i_ipbuff_temp2 cic_q_ipbuff_temp2;
    cic_i_ipbuff_chan = cic_i_ipbuff_chan(1:end-1,:);   % Drop end row, may hold filler zeros
    cic_q_ipbuff_chan = cic_q_ipbuff_chan(1:end-1,:);   % Drop end row, may hold filler zeros
    if ShowPlots>=0
        for ii=1:n_carr
            figure; plot(real(scaled_yc_duc(1:size(cic_i_ipbuff_chan(1:end,ii),1),ii)));
            hold on; plot(cic_i_ipbuff_chan(1:end,ii),'r:'); hold off;
            figure; plot(imag(scaled_yc_duc(1:size(cic_q_ipbuff_chan(1:end,ii),1),ii)));
            hold on; plot(cic_q_ipbuff_chan(1:end,ii),'r:'); hold off;
        end
    end

    %% Check samples match
    failed=0;
    for ch=1:n_carr
        i_mismatches = 0; q_mismatches = 0;
        display(sprintf('Checking Carrier %d',ch));
        %for ii=1:size(cic_ipbuff_temp,1)
        for ii=1:size(cic_i_ipbuff_chan(1:end,1),1)
            if real(scaled_yc_duc(ii,ch)) ~= cic_i_ipbuff_chan(ii,ch)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('I value mismatch on Channel %d at position %d',ch,ii));
                end
                i_mismatches = i_mismatches+1;
            end
            if imag(scaled_yc_duc(ii,ch)) ~= cic_q_ipbuff_chan(ii,ch)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('Q value mismatch on Channel %d at position %d',ch,ii));
                end
                q_mismatches = q_mismatches+1;
            end
        end
        display(sprintf('Carrier %d: I mismatches = %d',ch,i_mismatches));
        display(sprintf('Carrier %d: Q mismatches = %d',ch,q_mismatches));
        display(sprintf('Carrier %d: Total mismatches = %d',ch,i_mismatches+q_mismatches));
        if (i_mismatches+q_mismatches)>0
            failed=1;
        end
    end
    if failed==1
        error('Output from CIC input buffer does not match Matlab model Scaler output!')
    else
        display(sprintf('Matched CIC input buffer output to Matlab model Scaler output! [Samples=%d]',size(cic_i_ipbuff_chan,1)));
        clear scaled_yc_duc cic_i_ipbuff_dout cic_q_ipbuff_dout cic_i_ipbuff_chan cic_q_ipbuff_chan;
    end
end
%% Clear variables
clear failed ch i_mismatches q_mismatches;

%% Analyze CIC sample output
if exist('cic_i_dout')
    load results/ycic_duc.mat
    %cic_start = min(find(cic_i_vout>0));
    cic_start = 693;   % From analyzing cic_vout

    %% Chop preceding zeroes and tail
    cic_i_temp = cic_i_dout(cic_start:end-2);
    cic_q_temp = cic_q_dout(cic_start:end-2);

    %% Adjust for rate change and clock oversampling
    cic_i_temp2 = [];
    cic_q_temp2 = [];
    for ii=1:m_cic*clk_os:size(cic_i_temp)-m_cic*n_carr
        %display(sprintf('Index %d',ii));
        cic_i_temp2 = [ cic_i_temp2; cic_i_temp(ii:ii+m_cic*n_carr-1) ];
        cic_q_temp2 = [ cic_q_temp2; cic_q_temp(ii:ii+m_cic*n_carr-1) ];
    end
    clear cic_i_temp cic_q_temp;

    %% Split CIC dout into channels and analyze
    cic_i_chan = zeros(ceil(length(cic_i_temp2)/n_carr),n_carr);
    cic_q_chan = zeros(ceil(length(cic_q_temp2)/n_carr),n_carr);
    cic_i_temp2 = [ cic_i_temp2 ; zeros(numel(cic_i_chan)-length(cic_i_temp2),1) ];
    cic_q_temp2 = [ cic_q_temp2 ; zeros(numel(cic_q_chan)-length(cic_q_temp2),1) ];
    for ii=1:n_carr
        cic_i_chan(:,ii) = cic_i_temp2(ii:n_carr:end);
        cic_q_chan(:,ii) = cic_q_temp2(ii:n_carr:end);
    end
    clear cic_i_temp2 cic_q_temp2;
    cic_i_chan = cic_i_chan(1:end-1,:);   % Drop end row, may hold filler zeros
    cic_q_chan = cic_q_chan(1:end-1,:);   % Drop end row, may hold filler zeros

    % Trim leading zeros from Matlab model output
    ycic_duc_x = ycic_duc(5:5+size(cic_i_chan(1:end,ii),1),:);

    if ShowPlots>=0
        for ii=1:n_carr
            figure; plot(real(ycic_duc_x(1:size(cic_i_chan(1:end,ii),1),ii)));
            hold on; plot(cic_i_chan(1:end,ii),'r:'); hold off;
            figure; plot(imag(ycic_duc_x(1:size(cic_q_chan(1:end,ii),1),ii)));
            hold on; plot(cic_q_chan(1:end,ii),'r:'); hold off;
        end
    end

    %% Check samples match
    failed=0;
    for ch=1:n_carr
        i_mismatches = 0; q_mismatches = 0;
        display(sprintf('Checking Carrier %d',ch));
        %for ii=1:size(cic_temp,1)
        for ii=1:size(cic_i_chan(1:end,1),1)
            if real(ycic_duc_x(ii,ch)) ~= cic_i_chan(ii,ch)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('I value mismatch on Channel %d at position %d',ch,ii));
                end
                i_mismatches = i_mismatches+1;
            end
            if imag(ycic_duc_x(ii,ch)) ~= cic_q_chan(ii,ch)
                if (i_mismatches+q_mismatches)<50
                    display(sprintf('Q value mismatch on Channel %d at position %d',ch,ii));
                end
                q_mismatches = q_mismatches+1;
            end
        end
        display(sprintf('Carrier %d: I mismatches = %d',ch,i_mismatches));
        display(sprintf('Carrier %d: Q mismatches = %d',ch,q_mismatches));
        display(sprintf('Carrier %d: Total mismatches = %d',ch,i_mismatches+q_mismatches));
        if (i_mismatches+q_mismatches)>0
            failed=1;
        end
    end
    clear ycic_duc_x;
    if failed==1
        error('Output from CIC does not match Matlab model CIC output!')
    else
        display(sprintf('Matched CIC output to Matlab model CIC output! [Samples=%d]',size(cic_i_chan,1)));