% test_dqpsk.m
% David Rowe March 2014
%
% Single sample/symbol DQPSK modem simulation to test modulating modem
% tx power based on speech energy.

1;

% main test function 

function sim_out = ber_test(sim_in)
    Fs = 8000;

    verbose          = sim_in.verbose;
    framesize        = sim_in.framesize;
    Ntrials          = sim_in.Ntrials;
    Esvec            = sim_in.Esvec;
    phase_offset     = sim_in.phase_offset;
    w_offset         = sim_in.w_offset;
    plot_scatter     = sim_in.plot_scatter;
    Rs               = sim_in.Rs;
    hf_sim           = sim_in.hf_sim;
    nhfdelay         = sim_in.hf_delay_ms*Rs/1000;
    hf_phase_only    = sim_in.hf_phase_only;
    hf_mag_only      = sim_in.hf_mag_only;
    Nc               = sim_in.Nc;
    symbol_amp       = sim_in.symbol_amp;

    bps              = 2;
    Nsymb            = framesize/bps;
    for k=1:Nc
      prev_sym_tx(k) = qpsk_mod([0 0]);
      prev_sym_rx(k) = qpsk_mod([0 0]);
    end

    rate = 1;

    % Init HF channel model from stored sample files of spreading signal ----------------------------------

    % convert "spreading" samples from 1kHz carrier at Fs to complex
    % baseband, generated by passing a 1kHz sine wave through PathSim
    % with the ccir-poor model, enabling one path at a time.
    
    Fc = 1000; M = Fs/Rs;
    fspread = fopen("../raw/sine1k_2Hz_spread.raw","rb");
    spread1k = fread(fspread, "int16")/10000;
    fclose(fspread);
    fspread = fopen("../raw/sine1k_2ms_delay_2Hz_spread.raw","rb");
    spread1k_2ms = fread(fspread, "int16")/10000;
    fclose(fspread);

    % down convert to complex baseband
    spreadbb = spread1k.*exp(-j*(2*pi*Fc/Fs)*(1:length(spread1k))');
    spreadbb_2ms = spread1k_2ms.*exp(-j*(2*pi*Fc/Fs)*(1:length(spread1k_2ms))');

    % remove -2000 Hz image
    b = fir1(50, 5/Fs);
    spread = filter(b,1,spreadbb);
    spread_2ms = filter(b,1,spreadbb_2ms);

    % discard first 1000 samples as these were near 0, probably as
    % PathSim states were ramping up

    spread    = spread(1000:length(spread));
    spread_2ms = spread_2ms(1000:length(spread_2ms));

    % decimate down to Rs

    spread = spread(1:M:length(spread));
    spread_2ms = spread_2ms(1:M:length(spread_2ms));

    % Determine "gain" of HF channel model, so we can normalise
    % carrier power during HF channel sim to calibrate SNR.  I imagine
    % different implementations of ccir-poor would do this in
    % different ways, leading to different BER results.  Oh Well!

    hf_gain = 1.0/sqrt(var(spread)+var(spread_2ms));

    % Start Simulation ----------------------------------------------------------------

    for ne = 1:length(Esvec)
        EsNodB = Esvec(ne);
        EsNo = 10^(EsNodB/10);
    
        variance = 1/EsNo;
         if verbose > 1
            printf("EsNo (dB): %f EsNo: %f variance: %f\n", EsNodB, EsNo, variance);
        end
        
        Terrs = 0;  Tbits = 0;

        tx_symb_log        = [];
        rx_symb_log        = [];
        noise_log          = [];
        sim_out.errors_log = [];

        % init HF channel

        hf_n = 1;
        hf_angle_log = [];
        hf_fading = ones(1,Nsymb);             % default input for ldpc dec
        hf_model = ones(Ntrials*Nsymb/Nc, Nc); % defaults for plotting surface

        sim_out.errors_log = [];
        sim_out.Nerrs = [];
        sim_out.snr_log = [];
        sim_out.hf_model_pwr = [];

        symbol_amp_index = 1;

        for nn = 1: Ntrials
                  
            tx_bits = round( rand( 1, framesize*rate ) );
 
            % modulate --------------------------------------------

            s = zeros(1, Nsymb);
            for i=1:Nc:Nsymb
              for k=1:Nc
                tx_symb = qpsk_mod(tx_bits(2*(i-1+k-1)+1:2*(i+k-1)));
                tx_symb *= prev_sym_tx(k);
                prev_sym_tx(k) = tx_symb;
                s(i+k-1) = symbol_amp(symbol_amp_index)*tx_symb;
              end
            end
            s_ch = s;
            symbol_amp_index++;

            % HF channel simulation  ------------------------------------
            
            if hf_sim

                % separation between carriers.  Note this is
                % effectively under samples at Rs, I dont think this
                % matters.  Equivalent to doing freq shift at Fs, then
                % decimating to Rs.

                wsep = 2*pi*(1+0.5);  % e.g. 75Hz spacing at Rs=50Hz, alpha=0.5 filters

                if Nsymb/Nc != floor(Nsymb/Nc)
                    printf("Error: Nsymb/Nc must be an integrer\n")
                    return;
                end

                % arrange symbols in Nsymb/Nc by Nc matrix

                for i=1:Nc:Nsymb

                    % Determine HF channel at each carrier for this symbol

                    for k=1:Nc
                        hf_model(hf_n, k) = hf_gain*(spread(hf_n) + exp(-j*k*wsep*nhfdelay)*spread_2ms(hf_n));
                        hf_fading(i+k-1) = abs(hf_model(hf_n, k));
                        if hf_mag_only
                             s_ch(i+k-1) *= abs(hf_model(hf_n, k));
                        else
                             s_ch(i+k-1) *= hf_model(hf_n, k);
                        end
                    end
                    hf_n++;
                end
            end
            
            tx_symb_log = [tx_symb_log s_ch];

            % "genie" SNR estimate 
            
            snr = (s_ch*s_ch')/(Nsymb*variance);
            sim_out.snr_log = [sim_out.snr_log snr];
            sim_out.hf_model_pwr = [sim_out.hf_model_pwr mean(hf_fading.^2)];

            % AWGN noise and phase/freq offset channel simulation
            % 0.5 factor ensures var(noise) == variance , i.e. splits power between Re & Im

            noise = sqrt(variance*0.5)*(randn(1,Nsymb) + j*randn(1,Nsymb));
            noise_log = [noise_log noise];
 
            % organise into carriers to apply frequency and phase offset

            for i=1:Nc:Nsymb
              for k=1:Nc
                 s_ch(i+k-1) = s_ch(i+k-1)*exp(j*phase_offset) + noise(i+k-1);
              end 
              phase_offset += w_offset;
            end
               
            % de-modulate

            rx_bits = zeros(1, framesize);
            for i=1:Nc:Nsymb
              for k=1:Nc
                rx_symb = s_ch(i+k-1);
                tmp = rx_symb;
                rx_symb *= conj(prev_sym_rx(k)/abs(prev_sym_rx(k)));
                prev_sym_rx(k) = tmp;
                rx_bits((2*(i-1+k-1)+1):(2*(i+k-1))) = qpsk_demod(rx_symb);
                rx_symb_log = [rx_symb_log rx_symb];
              end
            end

            error_positions = xor(rx_bits, tx_bits);
            Nerrs = sum(error_positions);
            sim_out.Nerrs = [sim_out.Nerrs Nerrs];
            Terrs += Nerrs;
            Tbits += length(tx_bits);
            
            sim_out.errors_log = [sim_out.errors_log error_positions];
        end

        TERvec(ne) = Terrs;
        BERvec(ne) = Terrs/Tbits;

        if verbose 
            printf("EsNo (dB): %f  Terrs: %d BER %f ", EsNodB, Terrs, Terrs/Tbits);
            printf("\n");
        end
        if verbose > 1
            printf("Terrs: %d BER %f C %f N %f Es %f No %f Es/No %f\n\n", Terrs,
                   Terrs/Tbits, var(tx_symb_log), var(noise_log),
                   var(tx_symb_log), var(noise_log), var(tx_symb_log)/var(noise_log));
        end
    end
    
    Ebvec = Esvec - 10*log10(bps);

    sim_out.BERvec = BERvec;
    sim_out.Ebvec  = Ebvec;
    sim_out.TERvec = TERvec;

    if plot_scatter
        figure(2);
        clf;
        scat = rx_symb_log .* exp(j*pi/4);
        plot(real(scat), imag(scat),'+');
        title('Scatter plot');

        figure(3);
        clf;        
        y = 1:Rs*2;
        x = 1:Nc;
        EsNodBSurface = 20*log10(abs(hf_model(y,:))) - 10*log10(variance);
        mesh(x,y,EsNodBSurface);
        grid
        title('HF Channel Es/No');

        if 0 
        figure(4);
        clf;
        subplot(211)
        plot(y,abs(hf_model(y,1)))
        title('HF Channel Carrier 1 Mag');
        subplot(212)
        plot(y,angle(hf_model(y,1)))
        title('HF Channel Carrier 1 Phase');
        end
    end

endfunction

% Gray coded QPSK modulation function

function symbol = qpsk_mod(two_bits)
    two_bits_decimal = sum(two_bits .* [2 1]); 
    switch(two_bits_decimal)
        case (0) symbol =  1;
        case (1) symbol =  j;
        case (2) symbol = -j;
        case (3) symbol = -1;
    endswitch
endfunction

% Gray coded QPSK demodulation function

function two_bits = qpsk_demod(symbol)
    if isscalar(symbol) == 0
        printf("only works with scalars\n");
        return;
    end
    bit0 = real(symbol*exp(j*pi/4)) < 0;
    bit1 = imag(symbol*exp(j*pi/4)) < 0;
    two_bits = [bit1 bit0];
endfunction

function sim_in = standard_init
  sim_in.verbose          = 1;
  sim_in.plot_scatter     = 0;

  sim_in.Esvec            = 5:15; 
  sim_in.Ntrials          = 100;
  sim_in.framesize        = 64;
  sim_in.Rs               = 100;
  sim_in.Nc               = 8;

  sim_in.phase_offset     = 0;
  sim_in.w_offset         = 0;
  sim_in.phase_noise_amp  = 0;

  sim_in.hf_delay_ms      = 2;
  sim_in.hf_sim           = 0;
  sim_in.hf_phase_only    = 0;
  sim_in.hf_mag_only      = 0;
endfunction

function awgn_hf_ber_curves()
  sim_in = standard_init();

  Ebvec = sim_in.Esvec - 10*log10(2);
  BER_theory = 0.5*erfc(sqrt(10.^(Ebvec/10)));

  dpsk_awgn = ber_test(sim_in);
  sim_in.hf_sim           = 1;
  dpsk_hf   = ber_test(sim_in);

  figure(1); 
  clf;
  semilogy(Ebvec, BER_theory,'r;QPSK theory;')
  hold on;
  semilogy(dpsk_awgn.Ebvec, dpsk_awgn.BERvec,'g;DQPSK;')
  semilogy(dpsk_hf.Ebvec, dpsk_hf.BERvec,'g;DQPSK HF;')
  hold off;
  xlabel('Eb/N0')
  ylabel('BER')
  grid("minor")
  axis([min(Ebvec) max(Ebvec) 1E-3 1])
end

sim_in = standard_init();

% energy file sampled every 10ms

load ../src/ve9qrp.txt
pdB=10*log10(ve9qrp);
for i=1:length(pdB)
  if pdB(i) < 0
    pdB(i) = 0;
  end
end

% Down sample to 40ms rate used for 1300 bit/s codec, every 4th sample is transmitted

pdB = pdB(4:4:length(pdB));

% Use linear mapping function in dB domain to map to symbol power

power_map_x  = [ 0 20 24 40 50 ];
power_map_y  = [-6 -6  0 6  6];
mapped_pdB = interp1(power_map_x, power_map_y, pdB);

%sim_in.symbol_amp = 10 .^ (mapped_pdB/20);
sim_in.symbol_amp = ones(1,length(pdB));
sim_in.plot_scatter = 1;
sim_in.verbose      = 2;
sim_in.hf_sim       = 1;
sim_in.Esvec        = 10;
sim_in.Ntrials      = 400;

dqpsk_pwr_hf = ber_test(sim_in);

% note: need way to test that power is aligned with speech

figure(4)
clf;
plot((1:sim_in.Ntrials)*80*4, pdB(1:sim_in.Ntrials));
hold on;
plot((1:sim_in.Ntrials)*80*4, mapped_pdB(1:sim_in.Ntrials),'r');
hold off;

figure(5)
clf;

s = load_raw("../raw/ve9qrp.raw");
M=320; M_on_2 = M/2; % processing delay between input speech and centre of analysis window
plot(M_on_2:(M_on_2-1+sim_in.Ntrials*M),s(1:sim_in.Ntrials*M))
hold on;
plot((1:sim_in.Ntrials)*M, 5000*sim_in.symbol_amp(1:sim_in.Ntrials),'r');
hold off;
axis([1 sim_in.Ntrials*M -3E4 3E4]);

figure(6)
clf;
plot((1:sim_in.Ntrials)*M, 20*log10(sim_in.symbol_amp(1:sim_in.Ntrials)),'b;Es (dB);');
hold on;
plot((1:sim_in.Ntrials)*M, 10*log10(dqpsk_pwr_hf.hf_model_pwr),'g;Fading (dB);');
plot((1:sim_in.Ntrials)*M, 10*log10(dqpsk_pwr_hf.snr_log),'r;Es/No (dB);');

ber = dqpsk_pwr_hf.Nerrs/sim_in.framesize;
ber_clip = ber;
ber_clip(find(ber > 0.2)) = 0.2;
plot((1:sim_in.Ntrials)*M, -20+100*ber_clip,'k;BER (0-20%);');
hold off;
axis([1 sim_in.Ntrials*M -20 20])

fep=fopen("dqpsk_errors_pwr.bin","wb"); fwrite(fep, dqpsk_pwr_hf.errors_log, "short"); fclose(fep);
fber=fopen("ber.bin","wb"); fwrite(fber, ber, "float"); fclose(fber);
