% cost_init() - initialize time-varying Gauss or Gauss-Markov ISI FIR and G
%
function cost_init()

global trans;
global states;
global FIR;
global FIR_save;
global FIR_length;
global FIR_length_p;
global Memory;

global FIR_M;
global FIR_M_t;
global FIR_M_p_;

global G;
global avg_h;

global DD;
global qq;
global alpha;
global alpha2;

global N;
global B;
global inv_Rh;
global inv_Rh_d;
global var_noise;
global z;

% disp('cost_init');

% Note: inv_Rh here is inv(C) in the paper

if( isempty(B) ) % called from x4_em_init

  FIR = FIR_save(:,1);

  if( ~isempty(G) ) % this shouldn't happen.

    error( 'B empty, G not, in cost_init');

  end

else

  if( isempty(G) ) % deterministic

      FIR = B\(z');

  elseif( size(avg_h,2) == 1)	% Gauss-Markov

    DD = zeros( FIR_length_p, N*FIR_length_p);
    qq = zeros( FIR_length_p, N);
    Fv = [1:FIR_length_p];

    for k=N:-1:2

      if( length(var_noise) > 1)
        vn = var_noise(k);
      else
        vn = var_noise;
      end

      t = B(k,:)';
      D = FIR_M_t*t*((t'/vn)*FIR_M) + inv_Rh;
      q = FIR_M_t*t*((z(k) - t' * FIR_M_p_)/vn) + inv_Rh_d;
      if( k < N)
        CDk1 = inv_Rh * DD(:,Fv+k*FIR_length_p);
        D = D + alpha2*(inv_Rh - CDk1*inv_Rh);
        q = q + alpha*(CDk1*qq(:,k+1) - inv_Rh_d);
      end
      D = inv(D);
      DD(:,Fv+(k-1)*FIR_length_p) = D;
      qq(:,k) = q;
    end

    % k = 1
    %
    vn = var_noise(1);
    t = B(1,:)';
    CDk1 = inv_Rh * D;
    q = FIR_M_t*t*((z(1) - t' * FIR_M_p_)/vn) + inv_Rh_d + alpha*(CDk1*q); % RHS D and q are from k=2
    D = FIR_M_t*t*((t'/vn)*FIR_M) + inv_Rh - alpha2*(CDk1*inv_Rh);
    D = inv(D);
    d = D*q;
    G = FIR_M * D * FIR_M_t;
    FIR = FIR_M * d + FIR_M_p_;

  else	% time-varying Gaussian

    vn = var_noise(1);
    t = B(1,:)';
    D = inv( FIR_M_t*t*((t'/vn)*FIR_M) + inv_Rh);
    G = FIR_M * D * FIR_M_t;
    d = D * (FIR_M_t*t*(z(1) - t' * FIR_M_p_)/vn + inv_Rh*avg_h(:,1));
    FIR = FIR_M * d + FIR_M_p_;

  end

end

trans = FSM_trans( states, FIR);

% FIR
