% function [At, OA, Ruupp] = fixin(Ruutt, Ct, tol)
%
%--------------------------------------------------------------------------
% xt->xp, ut->up (Reducing ranking of ut->up, possible b/c Rxxtt singlar)
% Inputs: Ruutt, Ct, tol
% Outputs: At, OA, Ruupp
%
% Ruutt: autocorrelation matrix of ut
% Ct: "nullspace of H"-adjusted discrete modulator matrix
% tol: used in licols to determine rank of matrix Ctemp
% At: "nullspace of H & Rxx singularity"-adjusted discrete modulator matrix
% OA: projection matrix of A2 onto A
% Ruupp: autocorrelation matrix of up
%--------------------------------------------------------------------------

function [At, OA, Ruupp] = fixin(Ruutt, Ct, tol)
if nargin < 3
  tol = 1e-10;
end

% Step 1
Rxxtt = Ct * Ruutt * Ct'; 
% This is necessary to get V orthonormal. eig in MATLAB only produces
% orthonormal V when Rxxtt is known to be Hermitian. Without following
% command, MATLAB doesn't recognize Rxxtt as Hermitian.
Rxxtt = 0.5 * (Rxxtt + Rxxtt');
[V, D] = eig(Rxxtt);

% Step 2
eig_indices = abs(diag(D)) > tol;
Pq = V(:, eig_indices) * V(:, eig_indices)';

% Step 3
Atemp = Pq * Ct;
[At, A_indices] =licols(Atemp, tol);


% Getting the indices in A2 (This can easily be O(n) instead of O(n^2)
A2_indices = 1:size(Atemp, 2);
for A_index = A_indices
    A2_indices(A2_indices==A_index) = 0;
end

% Computing A2, OA, and Ruutt
A2_indices = A2_indices(A2_indices~=0);
A2 = Atemp(:, A2_indices);

% Step 4
OA = inv(At' * At) * (At' * A2);

% Step 5
R11tt = Ruutt(A_indices, A_indices);
R12tt = Ruutt(A_indices, A2_indices);
R21tt = Ruutt(A2_indices, A_indices);
R22tt = Ruutt(A2_indices, A2_indices);

% Step 8
Ruupp = R11tt + OA * R21tt + R12tt * OA' + OA * R22tt * OA';

end

