% FG.m, simulations related to the paper by
% Fredrik Gustafsson, IEEE Trans. Comm. 1995
%
% Gauss-Markov with alpha=1

global N;
global Memory;

global zstate;
global zstate_0;
global states;

format short g;


 SNR_array =    [ 0  5 10 15  20]'
 trials_array = [10 20 40 80 160]' * 30
%trials_array = [10 20 40 80 160]' * 0.1 + 1

% initialize rand() and randn() with time-of-day
%
seed=floor(sum(100*clock))
rand( 'state', seed);
randn( 'state', seed);

% set FIR channel length, Memory, and zstate
%
FIR_length = 3

Memory = FIR_length - 1;      % channel memory size

states = FSM( [-1 1], Memory);

zstate = 1;                   % initial state
zstate_0 = 0;

% set N, number of samples
%
N = 8

% Gaussian part of the channel coefficients
%
var_h = 0.01
stdev_h = sqrt(var_h);

% uncorrelated h's
%
T = eye(FIR_length,FIR_length)

inv_T = inv(T)
inv_Rh = (T'*T)/var_h
Rh = inv(inv_Rh)

% generate all possible sequences
%
seq = FSM( [-1 1], N);

N_seq = size(seq,2);

% array's to save fg and gm costs and BER
%
gm = zeros(N_seq,1);
fg = zeros(N_seq,1);

BER_gm = zeros(length(SNR_array),1);
BER_fg = zeros(length(SNR_array),1);

% set FIR_save dimension
%
FIR_save = zeros(FIR_length,N);

% try a fixed FIR
%
FIR_0 = ones(FIR_length,1);
FIR_save = ones(FIR_length,N);

% Monte-Carlo
%
for SNR_index = 1:length(SNR_array)

  SNR = SNR_array(SNR_index);

  trials = trials_array(SNR_index);

  trial_count = 0;

  while( trial_count < trials)

    trial_count = trial_count + 1;

  % generate FIR
  %
%    while( 1)
%
%      % FIR_0 = 10 * inv_T * stdev_h * randn(FIR_length,1);
%
%      FIR_0 = ones(FIR_length,1) + inv_T * stdev_h * randn(FIR_length,1);
%
%      FIR_save(:,1) = FIR_0 + inv_T * stdev_h * randn(FIR_length,1);
%
%      for i=2:N
%        FIR_save(:,i) = FIR_save(:,i-1) + inv_T * stdev_h * randn(FIR_length,1);
%      end
%  
%      hnorm2 = sum(FIR_save .* FIR_save);
%
%      if( min(hnorm2) > 0.10 * max(hnorm2)) % found good coefficients
%        break;
%      end
%
%    end

  % generate symbols and noise
  %
    x = 2*(rand(1,N) > 0.5) - 1;        % input sequence

    xt = [fliplr(states(:,zstate)'), x];

    y = zeros(1,N);

    for i=1:N
      y(i) = xt(i+Memory:-1:i) * FIR_save(:,i);
    end

    var_noise = 10^(-SNR/10) * sum( sum(FIR_save.*FIR_save)) / N;

    noise = sqrt(var_noise).*randn(1,N);  % random noise

    z = y + noise;                        % signal plus noise

  % ML cost for Gauss-Markov h with known FIR_0 and avg_h = 0
  % and alpha = 1
  %
    for i = 1:N_seq

      b = seq(:,i);
      A = get_A_C( b);
      % A = get_A( b);
  
      t = A(1,:)';
      q = t*(z(1)/var_noise) + inv_Rh*FIR_0;
      % G = inv( t*(t'/var_noise) + 2*inv_Rh );
      % more stable as var_noise -> 0:
      C2 = Rh/2;
      G = C2 - (C2*t)*((t'*C2)/(var_noise + t'*C2*t));
      
      cost = - log(det(G)) - q'*G*q;
  
      for k = 2:N-1

        t = A(k,:)';
        q = t*z(k)/var_noise + inv_Rh*(G*q);
        % G = inv( t*(t'/var_noise) + 2*inv_Rh  - inv_Rh*G*inv_Rh );
        C2 = inv(2*inv_Rh - inv_Rh*G*inv_Rh);
        G = C2 - (C2*t)*((t'*C2)/(var_noise + t'*C2*t));
  
        cost = cost - log(det(G)) - q'*G*q;
      end      

      t = A(N,:)';
      q = t*z(N)/var_noise + inv_Rh*(G*q);
      % G = inv( t*(t'/var_noise) + inv_Rh - inv_Rh*G*inv_Rh );
      C2 = inv(inv_Rh - inv_Rh*G*inv_Rh);
      G = C2 - (C2*t)*((t'*C2)/(var_noise + t'*C2*t));
  
      cost = cost - log(det(G)) - q'*G*q;

      gm(i) = cost;

    end

    [s1 s2] = sort(gm);
    x_hat = seq(:,s2(1))';
    BER_gm(SNR_index) = BER_gm(SNR_index) + sum(x_hat ~= x);

  % ML cost from Gustafsson
  %
    for i = 1:N_seq

      b = seq(:,i);
      A = get_A_C( b);
      % A = get_A( b);

      % Note: for Eq(23) with t=0 we have no y(0) for the right-hand-side
      %
      FIR = FIR_0;
      P = Rh;

      cost = 0;

      for k=1:N

        t = A(k,:)';
        var = t'*P*t + var_noise;
        e = z(k) - t'*FIR;
        cost = cost + e*e/var + log(var);

        FIR = FIR + P*t*(e/var);
        P = P + (P*t)*((t'*P)/var) + Rh;

      end

      fg(i) = cost;

    end

    [s1 s2] = sort(fg);
    x_hat = seq(:,s2(1))';
    BER_fg(SNR_index) = BER_fg(SNR_index) + sum(x_hat ~= x);

  end % trials

  BER_gm(SNR_index) = BER_gm(SNR_index)/(N*trials);
  BER_fg(SNR_index) = BER_fg(SNR_index)/(N*trials);

end % SNR_array

disp('SNR_array BER_gm BER_fg');

[SNR_array BER_gm BER_fg]
