function [theta,d] = svd_esprit (X1,X2,d_in,tol)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  [theta,d] = svd_esprit (X1,X2,d_in,tol)               %
%                                                        %
%  This program uses the SVD-based Esprit                %
%  algorithm to estimate the directions of arrival       %
%  (DOAs) of some signals.                               %
%                                                        %
%  X1:     the columns are the time series data from     %
%          the first sensor in each pair.                %
%  X2:     the columns are the time series data from     %
%          the second sensor in each pair.               %
%  d_in:   the true number of signals.                   %
%          If duse=0, then the                           %
%          algorithm should estimate this value.         %
%  tol:    the tolerance for deciding that the remaining %
%          signals are noise.                            %
%                                                        %
%  theta:  row j contains the estimated DOAs for the     %
%             jth time.                                  %
%  d:      the estimated number of signals               %
%                                                        %
%  From: Your Homework Assignment Project 4              %
%  The DOA Problem: Coming at You                        %
%  Dianne P. O'Leary 07/03                               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the SVD of the data matrix.                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[m,n] = size(X1);

[U,sigma,W] = svd([X1;X2]);
ss = diag(sigma);
m2 = length(ss);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Estimate the number of signals if necessary.           %
% Do this by checking a tolerance on the singular values.%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (d_in == 0)
  mysum = sum(ss.^2);
  d = 0;
  while mysum > tol^2*(m2-d)
    d = d + 1;
    mysum = mysum - ss(d)^2;
  end
  if (d > m2)
     d = m2;
  end
else
  d = d_in;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% In our algorithm,                                      %
% C = W*[inv(sigma(1:d,1:d));zeros(n-d,d)];              %
% B = [inv(Del(1:d,1:d)),zeros(d,m-d)]*t';               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the phases from the eigenvalues of             %
% a matrix of singular vectors and then compute the      %
% angles theta.                                          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[t,Del,v] = svd([U(1:m,1:d),U(m+1:2*m,1:d)]);
Phi = eig(v(d+1:2*d,1:d)',v(1:d,1:d)');

Phi = sort(Phi); % necessary to pick up the d largest values

for j=1:d,
  theta(j) = asin(4*angle(Phi(j))/(2*pi))*180/pi;
end
theta = sort(theta); % necessary to make the graphs continuous