clc;
close all;
clear all;

F=3000;     %signal frequency 3khz

N=1000;     %no. of samples used
Fs=7000;   %sampling rate 
Ts=1/Fs;    %sampling Period
C=340;      %velocity of sound (m/sec)
Lambda=C/F;
dx=Lambda/2;
dy=Lambda/2;
theta=input('Enter the Elevation angle : ');
phi=input('Enter the Azimuth angle : ');
theta= theta * pi/180; 
phi= phi * pi/180;   
sigma=0.01;

% P=[dx dy 0; 2*dx dy 0 ; 3*dx dy 0; dx 2*dy 0; 2*dx 2*dy 0; 
%     3*dx 2*dy 0 ; dx 3*dy 0 ;2*dx 3*dy 0;3*dx 3*dy 0];
P=[-2*dx 2*dy 0; 0 2*dy 0 ; 2*dx 2*dy 0; -2*dx 0 0; 0 0 0; 
    2*dx 0 0 ; -2*dx -2*dy 0 ;0 2*dy 0; 2*dx 2*dy 0];
K=[cos(theta)*sin(phi); sin(theta)*sin(phi);cos(phi)];
% V=zeros(9,1);
% for i=1:9
%     V(i,1)=exp(-j* P(i,:)*K); % Array Manifold Vector
% end

V=calcV(P,K);

amp1(1)=1;
phase1(1)=wgn(1,1,1,'linear');
s=wgn(1,8*N,0.01,'linear');

ind=0;
X=zeros(9,1000);
% for t=0:Ts:(N-1)*Ts
for n=1:N
      amp1(n+1) = amp1(n)+s(n);
      phase1(n+1)=phase1(n)+s(n);
    S=amp1(n)*exp(-j*2*pi*F*n+phase1(n));
    ind=ind+1;
    noise=wgn(9,1,sigma,'linear','complex');
    X(:,ind)= S*V+noise;
    n=n+1/Fs;
end
% HX=hilbert(X);
RX=X*conj(X')/N;
[U S v]=svd(RX);
Us=U(:,1);
Un=U(:,2:9);

Nx=100;
Ny=100;
theta=linspace(-pi/2,pi/2,Nx);
phi=linspace(-pi/2,pi/2,Ny);

Q=zeros(length(theta),length(phi));
for X=1:length(theta)
    for Y=1:length(phi)
        K=[cos(theta(X))*sin(phi(Y)); sin(theta(X))*sin(phi(Y));cos(phi(Y))];
        V=calcV(P,K);
        Q(X,Y)=1/(V'*Un*Un'*V);
    end
end

surf(theta*180/pi,phi*180/pi,abs(Q));
xlabel('phi (deg)');
ylabel('theta (deg)')