function [p_vec, sigma, R] = samv(y, K) %y: signal, K: max frequency tested (fs/2)

N = length(y);

omega_grid = linspace(0,1,K).';

A = exp(1j * (0:N-1).' * 2 * pi * omega_grid(:).');

p_vec_init = sum(abs(A'*y), 2 )/N;

sigma = 1/N*(y'*y);

R_N = 1/N * (y*y');

p_vec_old = abs(p_vec_init).^2;
threshold = 1e-6;

maxIter=8;
iterIdx=0;
for i = 1:maxIter
    iterIdx = iterIdx + 1;
    
    % Sparse matrix formed from diagonals : spdiags
    R =  A * spdiags(p_vec_old, 0, K, K) * A' + sigma*eye(N);

    Rinv=inv(R);
    Rinv_A = Rinv*A;
    
    diag_A_Rinv_A = sum(conj(A).*Rinv_A, 1).';
    beta = diag_A_Rinv_A.^2;
    num = sum(conj(A).* (Rinv*R_N*Rinv*A), 1).';
    
    p_vec = num./beta;
    
    % Sum of diagonal elements.
    sigma = real(trace(Rinv*Rinv*R_N))/real(trace(Rinv*Rinv));
    
    p_diffs_ratio = norm(p_vec_old-p_vec)/norm(p_vec_old);
    if p_diffs_ratio < threshold
        disp(['converges at iteration ' num2str(iterIdx)]);
        break;
    end
    
    if i==maxIter
        disp('reached number of maximum iterations');
    end
    
    p_vec_old = p_vec;
end
p_vec = abs(p_vec);
