% cohpsk_dev.m
% David Rowe Mar 2015
%
% Coherent PSK modem development and testing functions
%

cohpsk_lib;

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

function [spread spread_2ms hf_gain] = init_hf_model(Fs, nsam)

    % convert "spreading" samples from 1kHz carrier at Fss 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; Fss = 8000;
    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/Fss)*(1:length(spread1k))');
    spreadbb_2ms = spread1k_2ms.*exp(-j*(2*pi*Fc/Fss)*(1:length(spread1k_2ms))');

    % remove -2000 Hz image
    b = fir1(50, 5/Fss);
    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));

    % change output samples so they are at rate Fs reqd by caller
    
    spread = resample(spread, Fs, Fss);
    spread_2ms = resample(spread_2ms, Fs, Fss);

    % 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(1:nsam))+var(spread_2ms(1:nsam)));
endfunction


function write_pilot_file(pilot, Nsymbrowpilot, Ns, Nsymrow, Npilotsframe, Nc);

  filename = sprintf("../src/cohpsk_defs.h", Npilotsframe, Nc);
  f=fopen(filename,"wt");
  fprintf(f,"/* Generated by write_pilot_file() Octave function */\n\n");
  fprintf(f,"#define NSYMROW      %d   /* number of data symbols on each row (i.e. each carrier) */\n", Nsymrow);
  fprintf(f,"#define NS           %d   /* number of data symbols between pilots                   */\n", Ns);
  fprintf(f,"#define NPILOTSFRAME %d   /* number of pilot symbols on each row                     */\n", Npilotsframe);
  fprintf(f,"#define PILOTS_NC    %d   /* number of carriers                                      */\n\n", Nc);
  fprintf(f,"#define NSYMROWPILOT %d   /* length of row after pilots inserted                     */\n\n", Nsymbrowpilot);
  fclose(f);

  filename = sprintf("../src/pilots_coh.h", Npilotsframe, Nc);
  f=fopen(filename,"wt");
  fprintf(f,"/* Generated by write_pilot_file() Octave function */\n\n");
  fprintf(f,"float pilots_coh[][PILOTS_NC]={\n");
  for r=1:Npilotsframe
    fprintf(f, "  {");
    for c=1:Nc-1
      fprintf(f, "  %f,", pilot(r, c));
    end
    if r < Npilotsframe
      fprintf(f, "  %f},\n", pilot(r, Nc));
    else
      fprintf(f, "  %f}\n};", pilot(r, Nc));
    end
  end
  fclose(f);
endfunction


% Save test bits frame to a text file in the form of a C array

function test_bits_coh_file(test_bits_coh)

  f=fopen("../src/test_bits_coh.h","wt");
  fprintf(f,"/* Generated by test_bits_coh_file() Octave function */\n\n");
  fprintf(f,"const int test_bits_coh[]={\n");
  for m=1:length(test_bits_coh)-1
    fprintf(f,"  %d,\n",test_bits_coh(m));
  endfor
  fprintf(f,"  %d\n};\n",test_bits_coh(length(test_bits_coh)));
  fclose(f);

endfunction


% Rate Rs BER tests ------------------------------------------------------------------------------

function sim_out = ber_test(sim_in)
    sim_in = symbol_rate_init(sim_in);

    Fs               = sim_in.Fs;
    Rs               = sim_in.Rs;
    Ntrials          = sim_in.Ntrials;
    verbose          = sim_in.verbose;
    plot_scatter     = sim_in.plot_scatter;
    framesize        = sim_in.framesize;
    bps              = sim_in.bps;

    Esvec            = sim_in.Esvec;
    ldpc_code        = sim_in.ldpc_code;
    rate             = sim_in.ldpc_code_rate;
    code_param       = sim_in.code_param;
    tx_bits_buf      = sim_in.tx_bits_buf;
    Nsymb            = sim_in.Nsymb;
    Nsymbrow         = sim_in.Nsymbrow;
    Nsymbrowpilot    = sim_in.Nsymbrowpilot;
    Nc               = sim_in.Nc;
    Npilotsframe     = sim_in.Npilotsframe;
    Ns               = sim_in.Ns;
    Np               = sim_in.Np;
    Nd               = sim_in.Nd;
    modulation       = sim_in.modulation;
    pilot            = sim_in.pilot;
    prev_sym_tx      = sim_in.prev_sym_tx;
    prev_sym_rx      = sim_in.prev_sym_rx;
    rx_symb_buf      = sim_in.rx_symb_buf;
    tx_pilot_buf     = sim_in.tx_pilot_buf;
    rx_pilot_buf     = sim_in.rx_pilot_buf;

    hf_sim           = sim_in.hf_sim;
    nhfdelay         = sim_in.hf_delay_ms*Rs/1000;
    hf_mag_only      = sim_in.hf_mag_only;
    f_off            = sim_in.f_off;
    div_time_shift   = sim_in.div_timeshift;

    [spread spread_2ms hf_gain] = init_hf_model(Rs, Nsymbrowpilot*(Ntrials+2));

    if strcmp(modulation,'dqpsk')
      Nsymbrowpilot = Nsymbrow;
    end

    % 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;

        s_ch_tx_log      = [];
        rx_symb_log      = [];
        noise_log        = [];
        errors_log       = [];
        Nerrs_log        = [];
        phi_log          = [];
        amp_log          = [];
        EsNo__log        = [];

        ldpc_errors_log = []; ldpc_Nerrs_log = [];

        Terrsldpc = Tbitsldpc = Ferrsldpc = 0;

        % init HF channel

        hf_n = 1;

        phase_offset_rect = 1;
        w_offset      = 2*pi*f_off/Rs;
        w_offset_rect = exp(j*w_offset);

        ct_symb_buf = zeros(2*Nsymbrowpilot, Nc*Nd);
        prev_tx_symb = prev_rx_symb = ones(1, Nc*Nd);

        % simulation starts here-----------------------------------
 
        for nn = 1:Ntrials+2
                  
            if ldpc_code
              tx_bits = round(rand(1,framesize*rate));                       
            else
              tx_bits = round(rand(1,framesize));                       
            end

            if strcmp(modulation,'qpsk')
              [tx_symb tx_bits] = bits_to_qpsk_symbols(sim_in, tx_bits, code_param);

              % one frame delay on bits for qpsk

              tx_bits_buf(1:framesize) = tx_bits_buf(framesize+1:2*framesize);
              tx_bits_buf(framesize+1:2*framesize) = tx_bits;

            end
            if strcmp(modulation, 'dqpsk')
              [tx_symb prev_tx_symb] = bits_to_dqpsk_symbols(sim_in, tx_bits, prev_tx_symb);
              tx_bits_buf(1:framesize) = tx_bits;
            end

            s_ch = tx_symb;

            % HF channel simulation  ------------------------------------
            
            hf_fading = ones(1,Nsymb);
            if hf_sim

                % separation between carriers.  Note this 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

                hf_model(hf_n, :) = zeros(1,Nc*Nd);
                
                for r=1:Nsymbrowpilot
                  for c=1:Nd*Nc
                    if c > Nc 
                      time_shift = sim_in.div_timeshift;
                    else
                      time_shift = 1;
                    end
                    ahf_model = hf_gain*(spread(hf_n+time_shift) + exp(-j*c*wsep*nhfdelay)*spread_2ms(hf_n+time_shift));
                    
                    if hf_mag_only
                      s_ch(r,c) *= abs(ahf_model);
                    else
                      s_ch(r,c) *= ahf_model;
                    end
                    hf_model(hf_n, c) = ahf_model;
                  end
                  hf_n++;
                end
            end
           
            % keep a record of each tx symbol so we can check average power

            for r=1:Nsymbrow
              for c=1:Nd*Nc
                 s_ch_tx_log = [s_ch_tx_log s_ch(r,c)];
              end
            end

            % 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(Nsymbrowpilot,Nc*Nd) + j*randn(Nsymbrowpilot,Nc*Nd));
            noise_log = [noise_log noise];

            for r=1:Nsymbrowpilot
              s_ch(r,:) *= phase_offset_rect;
              phase_offset_rect *= w_offset_rect;
            end
            s_ch += noise;

            ct_symb_buf(1:Nsymbrowpilot,:) = ct_symb_buf(Nsymbrowpilot+1:2*Nsymbrowpilot,:);
            ct_symb_buf(Nsymbrowpilot+1:2*Nsymbrowpilot,:) = s_ch;

            if strcmp(modulation,'qpsk')
              [rx_symb rx_bits rx_symb_linear amp_ phi_ sig_rms noise_rms sim_in] = qpsk_symbols_to_bits(sim_in, ct_symb_buf(1:Nsymbrowpilot+Npilotsframe,:));                                 
              phi_log = [phi_log; phi_];
              amp_log = [amp_log; amp_];
            end
            if strcmp(modulation,'dqpsk')
              [rx_symb rx_bits rx_symb_linear prev_rx_symb] = dqpsk_symbols_to_bits(sim_in, s_ch, prev_rx_symb);                                 
            end
                        
            % Wait until we have enough frames to do pilot assisted phase estimation

            if nn > 1
              rx_symb_log = [rx_symb_log rx_symb_linear];
              %EsNo__log = [EsNo__log EsNo_];

              % Measure BER

              error_positions = xor(rx_bits, tx_bits_buf(1:framesize));
              Nerrs = sum(error_positions);
              Terrs += Nerrs;
              Tbits += length(tx_bits);
              errors_log = [errors_log error_positions];
              Nerrs_log = [Nerrs_log Nerrs];

              % Optionally LDPC decode
            
              if ldpc_code
                detected_data = ldpc_dec(code_param, sim_in.max_iterations, sim_in.demod_type, sim_in.decoder_type, ...
                                         rx_symb_linear, min(100,EsNo_), amp_linear);
                error_positions = xor( detected_data(1:framesize*rate), tx_bits_buf(1:framesize*rate) );
                Nerrs = sum(error_positions);
                ldpc_Nerrs_log = [ldpc_Nerrs_log Nerrs];
                ldpc_errors_log = [ldpc_errors_log error_positions];
                if Nerrs
                    Ferrsldpc++;
                end
                Terrsldpc += Nerrs;
                Tbitsldpc += framesize*rate;
              end
            end
          end
           
          TERvec(ne) = Terrs;
          BERvec(ne) = Terrs/Tbits;

            if verbose 
              av_tx_pwr = (s_ch_tx_log * s_ch_tx_log')/length(s_ch_tx_log);

              printf("EsNo (dB): %3.1f Terrs: %d Tbits: %d BER %5.3f QPSK BER theory %5.3f av_tx_pwr: %3.2f",
                     EsNodB, Terrs, Tbits,
                       Terrs/Tbits, 0.5*erfc(sqrt(EsNo/2)), av_tx_pwr);
              if ldpc_code
                  printf("\n LDPC: Terrs: %d BER: %4.2f Ferrs: %d FER: %4.2f", 
                         Terrsldpc, Terrsldpc/Tbitsldpc, Ferrsldpc, Ferrsldpc/Ntrials);
              end
              printf("\n");
            end
    end
    
    Ebvec = Esvec - 10*log10(bps);
    sim_out.BERvec          = BERvec;
    sim_out.Ebvec           = Ebvec;
    sim_out.TERvec          = TERvec;
    sim_out.errors_log      = errors_log;
    sim_out.ldpc_errors_log = ldpc_errors_log;

    if plot_scatter
        figure(2);
        clf;
        scat = rx_symb_log .* exp(j*pi/4);
        plot(real(scat), imag(scat),'+');
        title('Scatter plot');
        a = 1.5*max(real(scat)); b = 1.5*max(imag(scat));
        axis([-a a -b b]);

        if hf_sim
          figure(3);
          clf;
        
          y = 1:(hf_n-1);
          x = 1:Nc*Nd;
          EsNodBSurface = 20*log10(abs(hf_model(y,:))) - 10*log10(variance);
          EsNodBSurface(find(EsNodBSurface < -5)) = -5;
          EsNodBSurface(find(EsNodBSurface > 25)) = 25;
          mesh(x,y,EsNodBSurface);
          grid
          axis([1 Nc*Nd 1 Rs*5 -5 25])
          title('HF Channel Es/No');

          if verbose 
            [m n] = size(hf_model);
            av_hf_pwr = sum(sum(abs(hf_model(:,:)).^2))/(m*n);
            printf("average HF power: %3.2f over %d symbols\n", av_hf_pwr, m*n);
          end

       end

       if strcmp(modulation,'qpsk')
          % set up time axis to include gaps for pilots

         [m1 n1] = size(phi_log);
         phi_x = [];
         phi_x_counter = 1;
         p = Ns;
         for r=1:m1
           if p == Ns
             phi_x_counter += Npilotsframe;
             p = 0;
           end
           p++;
           phi_x = [phi_x phi_x_counter++];        
         end

         phi_x -= Nsymbrowpilot; % account for delay in pilot buffer

         figure(5);
         clf
         subplot(211)
         [m n] = size(phi_log);
         plot(phi_x, phi_log(:,2),'r+;Estimated HF channel phase;')
         if hf_sim
           hold on;
           [m n] = size(hf_model);
           plot(angle(hf_model(1:m,2)),'g;HF channel phase;')
           hold off;
         end
         ylabel('Phase (rads)');
         legend('boxoff');
         axis([1 m -1.1*pi 1.1*pi])

         subplot(212)
         plot(phi_x, amp_log(:,2),'r+;Estimated HF channel amp;')
         if hf_sim
           hold on;
           plot(abs(hf_model(1:m,2)))
           hold off;
         end
         ylabel('Amplitude');
         xlabel('Time (symbols)');
         legend('boxoff');
         axis([1 m 0 3])
       end

       figure(4)
       clf
       stem(Nerrs_log)
       axis([1 length(Nerrs_log) 0 max(Nerrs_log)+1])
   end

endfunction

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

  sim_in.Esvec            = 50; 
  sim_in.Ntrials          = 30;
  sim_in.framesize        = 2;
  sim_in.Rs               = 50;

  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_mag_only      = 0;

  sim_in.Nd            = 1;
endfunction