%
% function [Ct, Ot, Ruutt] = fixmod(H_NW, Ruu, C, tol)
%--------------------------------------------------------------------------
% fixmod removes the part of x that lies in H_NW's nullspace
%       x->xt, u->ut 
% Inputs: H_NW, Ruu, C, tol
%     H_NW: Noise-whitened channel
%     Ruu: Autocorrelation matrix of u
%     C: discrete modulator matrix
%     tol: used in licols to determine rank of matrix Ctemp
%
% Outputs: Ct, Ot, Ruutt
%     Ct: new discrete modulator with components without H_NW's nullspace
%        components
%     Ot: Projection matrix of C2 onto C
%     Ruutt: new autocorrelation matrix for u
%
% This program calls licols
%--------------------------------------------------------------------------
function [Ct, Ot, Ruutt] = eliminate_x_nullspace(H_NW, Ruu, C, tol)

if nargin < 4
  tol = 1e-10;
end

% Step 1
[F, Lambda, M] = svd(H_NW);

% Step 2
Rho_H = rank(H_NW);
% Grabbing Rho_H vectors from M to form P_m
P_m = M(:, 1:Rho_H) * M(:, 1:Rho_H)';

% Step 3
Ctemp = P_m * C;

% Getting Ct from Ctemp
[Ct, Ct_indices] =licols(Ctemp, tol);


% Getting the indices in C2 (This can easily be O(n) instead of O(n^2)
C2_indices = 1:size(Ctemp, 2);
for Ct_index = Ct_indices
    C2_indices(C2_indices==Ct_index) = 0;
end

% Computing C2, Ot, and Ruutt
C2_indices = C2_indices(C2_indices~=0);
C2 = Ctemp(:, C2_indices);

% Step 4
Ot = inv(Ct' * Ct) * (Ct' * C2);

% Step 5
R11 = Ruu(Ct_indices, Ct_indices);
R12 = Ruu(Ct_indices, C2_indices);
R21 = Ruu(C2_indices, Ct_indices);
R22 = Ruu(C2_indices, C2_indices);

% Step 8
Ruutt = R11 + Ot * R21 + R12 * Ot' + Ot * R22 * Ot';
end

