%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Main function to do BroadBand Beamforming
function fBBB()
disp('Start fBB.m')
%Parameters to change.
desiredspeaker = 2; %Signal of interest, 1 is speaker 1, 2 is speaker 2

%Source coordinates
if desiredspeaker==1
    zInt_for_elevation = 0.0; %To set an elevation in the sources for beampattern
    src1coord = [-1.1000+0.75,-0.7500, 0.0];
    src2coord = [-0.40+0.75,-0.75 zInt_for_elevation];

elseif desiredspeaker==2
    zInt_for_elevation = 0.0; %To set an elevation in the sources for beampattern
    src1coord = [-0.40+0.75,-0.75 zInt_for_elevation];
    src2coord = [-1.1000+0.75,-0.7500, 0.0];
else
    disp('Error speaker number'), keyboard
end

mindist = 0.1; %Min distance to limit the amplitude
gainpower = 1;

%%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Element and source locations in cartesian coordinates
A = [-0.82; -0.75; -0.68; -0.65; -0.68; -0.75; -0.82; -0.85]+0.75; %A: x-coordinates of mics %Translate coordinates to the origin
B = [ 0.07;  0.10;  0.07;  0.00; -0.07; -0.10; -0.07;  0.00]; %B: y-coordinates of mics
array_ref = [0 0 0];  % Reference at origin of coordinates

%%   %Legacy, set the sources in matrices D and E.
%   SOI       Interf.
D = [src1coord(1); src2coord(1);]; %D: x-coordinates of speakers
E = [src1coord(2); src2coord(2);]; %E: y-coordinates of speakers
F = [src1coord(3); src2coord(3);]; %F: z-coordinates of speakers

srcpos   = [D E F]'; %xy coordinates of speakers each column has the xy coordinates
mpos     = [A B]'; %xy coordinates of mics, each column has the mic xy coord.
%the third coordinate in the third row is zero since the array is in the xy plane
mpos(3,:)=zeros(1,length(A));

save('..\data\mpos_srcpos', 'mpos','srcpos')

%%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%Interference and noise indexes to read from the beamformer output signal
intindexes.intstart = 12001;
intindexes.intend   = 16000;
intindexes.noisestart = 16017; %to create a block with only noise and find the Pn!!
intindexes.noiseend   = 20016;

%Generate signal with the new conditions, usually the new array_ref
[array_sig,Fs,sa_delay]=fInlgenerateSimSignal(mpos,srcpos,array_ref,desiredspeaker); %#ok<NASGU>
save('..\data\array_sig_dft_music', 'array_sig','intindexes')

%Load generated data
load('..\data\array_sig_dft_music', 'array_sig','intindexes');


%%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Plot the configuration, sources, and reference
fmicposLocal(mpos,srcpos,array_ref)

%Perform beamforming with the theoretical parameters
figure('Name','Input signal: speaker1');
plot(array_sig(1,:),'DisplayName',...
    'Speaker one','Marker','none','LineStyle','-','LineWidth',2,'Color','blue');
set(gca,'fontsize',12);
title('Input signal: from sensor 1')
legend(gca,'Location','SouthEast');
axis([0 length(array_sig(1,:)) -1 1])
grid on
%Save it in data\out directory
wavwrite(array_sig(1,:),Fs,32,'..\data\out\array_sig1.wav');
%keyboard
% Configuration of the circular array
N_array_els=size(array_sig,1); % number of array elements, read from rows of data
N_speakers=2;  % number of speakers
c=343; % speed of sound
FFT_len=256; %

%Time delays and gain for each speaker(source) and array elements, needed to compute the steering vectors
dist_src_ref = zeros(N_speakers,1);
gain_ref_mics = zeros(N_speakers,N_array_els);
tdelay_ref_mics = zeros(N_speakers,N_array_els);
for i_speaker = 1:N_speakers %For each speaker
    xspeaker = srcpos(1,i_speaker); %Absolute x coordinate of the source
    yspeaker = srcpos(2,i_speaker); %Absolute y coordinate of the source
    zspeaker = srcpos(3,i_speaker); %Absolute y coordinate of the source

    % Distance of the speakers to the array center
    xspeakref = xspeaker-array_ref(1);
    yspeakref = yspeaker-array_ref(2);
    zspeakref = zspeaker-array_ref(3);

    %Find the phi_SOI and phi_Int for this particular scenario
    %%%%%%%
    if i_speaker==1
        [phi_SOI,theta_SOI,rSOI]=fInlFindPhiSOIInt(i_speaker,xspeakref,yspeakref,zspeakref); %#ok<NASGU>
    elseif i_speaker==2
        [phi_Int,theta_Int,rInt]=fInlFindPhiSOIInt(i_speaker,xspeakref,yspeakref,zspeakref); %#ok<NASGU>
    end

    %3D Distance from the speaker source to the reference point
    dist_src_ref(i_speaker, 1) = sqrt(xspeakref^2 + yspeakref^2+ zspeakref^2);

    %Find distance from the src to the mics, works for 3D
    vectdiff = mpos.'-repmat(srcpos(:,i_speaker).',N_array_els,1);
    dist_src_mics=sqrt(sum(vectdiff.^2.'));

    %Find the gain coefficient in a row vector
    gain_ref_mics(i_speaker,:) = dist_src_ref(i_speaker, 1)./dist_src_mics;

    %Find the delay coefficient to each array elements
    dist_mics_ref = dist_src_mics-dist_src_ref(i_speaker, 1); %distances from the mics to the center in the direction of the source
    tdelay_ref_mics(i_speaker,:) = 1/c*(dist_mics_ref);
end %for i_speaker = 1:N_speakers %For each speaker

%Delay tau
tau_spk1=tdelay_ref_mics(1,:)';
tau_spk2=tdelay_ref_mics(2,:)';

%% Generate the steering vectors for all positive frequency bins (1/2 of all bins)
freq_bins=[0:FFT_len/2-1]/FFT_len*Fs; %Half of the FFT bins. This is for positive freqs.

%minfbinsindx = find(100 < freq_bins & freq_bins < 150) %Min index of the frequency bins to filter out low frequencies
minfbinsindx = max(2,find(freq_bins < 1, 1, 'last' )); %Min index of the frequency bins to filter out low frequencies
disp(['min fbin: ',num2str(freq_bins(minfbinsindx))]);
maxfbinsindx = min(length(freq_bins),find(7937< freq_bins, 1, 'first' ));
disp(['Max fbin: ',num2str(freq_bins(maxfbinsindx))]);
s_weights_spk1=zeros(N_array_els,FFT_len/2);
s_weights_spk2=zeros(N_array_els,FFT_len/2);
for i_freqbins=minfbinsindx:maxfbinsindx %For each frequency bin (why starting at i_freqbins=2?>>No DC)
    %Steering vectors are the same independent of the beamforming method
    for k_array_els =1:N_array_els %For each microphone sensor find the phase shift and >>>gain<<<
        s_weights_spk1(k_array_els,i_freqbins)=gain_ref_mics(1,k_array_els)^gainpower*exp(-j*2*pi*freq_bins(i_freqbins)*tau_spk1(k_array_els)); %Delay and Sum
        s_weights_spk2(k_array_els,i_freqbins)=gain_ref_mics(2,k_array_els)^gainpower*exp(-j*2*pi*freq_bins(i_freqbins)*tau_spk2(k_array_els)); %Delay and Sum
    end %for k_array_els =1:N_array_els
end %for i_freqbins=2:length(freq_bins)

%%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Compute the weight vector w(f). This is done once for fixed beamforming!!
v1=zeros(N_array_els,FFT_len/2);
for i_freqbins=minfbinsindx:maxfbinsindx %For each frequency bins!!
    %DAS weight vector
    v1(:,i_freqbins)=s_weights_spk1(:,i_freqbins)/norm(s_weights_spk1(:,i_freqbins))^2;
end %for i_freqbins=2:length(freq_bins)
%Check out the distortionless response!!
%current_freqbin = 32; %This is for 2kHz!!
%s_weights_spk1(:,32)'*v1(:,32)
%s_weights_spk2(:,32)'*v1(:,32)
%All the constraints are working as designed

%%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot the 3D beampattern, beampatterns, beam
current_freqbin = 32; %This is for 2kHz!!
current_freq = freq_bins(current_freqbin); %Current frequency to plot the bp
v_0 = v1(:,current_freqbin); %Weight vector

%Input parameters for the function to plot the 3D Nearfield Beampattern
pIn.current_freq=current_freq;   %Working frequency
pIn.v_0=v_0;                     %Weight column vector.
pIn.c=c;                         %propagation speed
pIn.mpos=mpos;                   %mic positions in columns. First row is x coordinate, second row y coordinate, third row z coordinate
pIn.array_ref=array_ref;       %Array reference in a row vector [xyz] coord, usually is the array center, or the origin of coordinates
pIn.srcpos=srcpos;               %Source position in a column vector [x;y;z]
pIn.mindist=mindist; %Minimum distance to limit the etc etc

fplotNFBeamp(pIn)
%keyboard
clear pIn; %Clear input parameters


%Plot the beampattern from the weights vector
phiresolution = 2*pi/600;
phiVec=0:phiresolution :2*pi; %scanning azimuth angle
current_freqbin = 32; %This is for 2kHz!!
current_freq = freq_bins(current_freqbin);
v_0 = v1(:,current_freqbin);
farfieldbp=0;
if farfieldbp %Create the far field beampattern
    theta_scan = 90/180*pi; %xy plane;
    %lambda = c/current_freq;
    sub_pos = [mpos' zeros(length(mpos),1)]; %This is absolute position???? <<<<Should be with respect the reference!!!
    disp('Check out sub_pos, has to be referenced to the ref point');keyboard
    %fmicposLocal(sub_pos',srcpos)
    bp=zeros(1,length(phiVec));
    for i_phiVec=1:length(phiVec)
        phi_scan=phiVec(i_phiVec);
        DOA_0=[sin(theta_scan)*cos(phi_scan),sin(theta_scan)*sin(phi_scan),cos(theta_scan)]';
        tdelay_ff = -sub_pos*DOA_0/c;
        s_scan1=exp(-j*2*pi*current_freq*tdelay_ff); %Column vector
        bp(i_phiVec)=abs(s_scan1'*v_0)^2;
    end; %for i_phiVec=1:length(phiVec),
else %Now is nearfield beampattern
    i_speakerbp =1;
    %disp(['Near field beampattern at speaker: ',num2str(i_speakerbp)])
    %The distance where we measure the nearfield beampatternis the same as the speaker1 to the reference
    % Distance of the source to the reference
    xscantoref = srcpos(1,i_speakerbp)-array_ref(1);
    yscantoref = srcpos(2,i_speakerbp)-array_ref(2);
    zscantoref = srcpos(3,i_speakerbp)-array_ref(3);
    %r_scan = 10^10; If you want to equate to far field
    r_scan = sqrt(xscantoref^2+yscantoref^2+zscantoref^2); %distance to scan
    theta_scan = acos(zscantoref/r_scan); %Scan at the elevation of the selected speaker
    bp=zeros(N_speakers,length(phiVec));
    for i_phiVec=1:length(phiVec) %Loop for the azimuth angle
        phi_scan=phiVec(i_phiVec);
        %Find the cartesian coordinates of the scanning when center is zero!!
        x_scan=r_scan*sin(theta_scan)*cos(phi_scan);
        y_scan=r_scan*sin(theta_scan)*sin(phi_scan);
        z_scan=r_scan*cos(theta_scan);
        pos_scan=[x_scan,y_scan,z_scan]; %scan coordinates in a row vector

        %Find the mic positions with respect the array reference
        mpos_ref(1,:)=mpos(1,:)-array_ref(1);
        mpos_ref(2,:)=mpos(2,:)-array_ref(2);
        mpos_ref(3,:)=mpos(3,:)-array_ref(3);

        %Find distance from the pos_scan to the mics
        vectdiff_scan = mpos_ref.'-repmat(pos_scan,N_array_els,1);
        dist_src_scan_mics=sqrt(sum(vectdiff_scan.^2.'));

        gain_ref_mic_scan = r_scan./max(mindist,dist_src_scan_mics);
        %if(min(dist_src_scan_mics)<mindist), disp('Limited to 1'); end;

        %Find the delay coefficient to each array elements
        dist_mics_ref = dist_src_scan_mics-r_scan; %distances from the mics to the center in the direction of the source
        tdelay_ref_mics = 1/c*(dist_mics_ref);

        %Find the near field steering vector
        s_scan_nearfield=(gain_ref_mic_scan.^gainpower.*exp(-j*2*pi*current_freq*tdelay_ref_mics)).'; %Column vector

        %Now find the beampattern
        bp(i_speakerbp,i_phiVec)=abs(s_scan_nearfield'*v_0)^2;
    end; %for i_phiVec=1:length(phiVec),

    i_speakerbp =2;
    %disp(['Near field beampattern at speaker: ',num2str(i_speakerbp)])
    %The distance where we measure the nearfield beampatternis the same as the speaker1 to the reference
    % Distance of the source to the reference
    xscantoref = srcpos(1,i_speakerbp)-array_ref(1);
    yscantoref = srcpos(2,i_speakerbp)-array_ref(2);
    zscantoref = srcpos(3,i_speakerbp)-array_ref(3);
    %r_scan = 10^10; If you want to equate to far field
    r_scan = sqrt(xscantoref^2+yscantoref^2+zscantoref^2); %distance to scan
    %theta_scan = 90/180*pi; %xy plane;
    theta_scan = acos(zscantoref/r_scan); %Scan at the elevation of the selected speaker

    for i_phiVec=1:length(phiVec), %Loop for the azimuth angle
        phi_scan=phiVec(i_phiVec);
        %Find the cartesian coordinates of the scanning when center is zero!!
        x_scan=r_scan*sin(theta_scan)*cos(phi_scan);
        y_scan=r_scan*sin(theta_scan)*sin(phi_scan);
        z_scan=r_scan*cos(theta_scan);
        pos_scan=[x_scan,y_scan,z_scan]; %scan coordinates in a row vector

        %Find the mic positions with respect the array reference
        mpos_ref(1,:)=mpos(1,:)-array_ref(1);
        mpos_ref(2,:)=mpos(2,:)-array_ref(2);
        mpos_ref(3,:)=mpos(3,:)-array_ref(3);

        %Find distance from the pos_scan to the mics
        vectdiff_scan = mpos_ref.'-repmat(pos_scan,N_array_els,1);
        dist_src_scan_mics=sqrt(sum(vectdiff_scan.^2.'));

        gain_ref_mic_scan = r_scan./max(mindist,dist_src_scan_mics);
        %if(min(dist_src_scan_mics)<mindist), disp('Limited to 1'); end;

        %Find the delay coefficient to each array elements
        dist_mics_ref = dist_src_scan_mics-r_scan; %distances from the mics to the center in the direction of the source
        tdelay_ref_mics = 1/c*(dist_mics_ref);

        %Find the near field steering vector
        s_scan_nearfield=(gain_ref_mic_scan.^gainpower.*exp(-j*2*pi*current_freq*tdelay_ref_mics)).'; %Column vector

        %Now find the beampattern
        bp(i_speakerbp,i_phiVec)=abs(s_scan_nearfield'*v_0)^2;
    end; %for i_phiVec=1:length(phiVec),

end %if farfieldbp, else
%%
plotdB = 1;
if plotdB
    if farfieldbp==1
        strNF= 'Far';
    else
        strNF= 'Near';
    end
    figure('Name',[strNF,' Field Beampattern']);
    hold on
    plot(phiVec/pi*180,10*log10(bp(1,:)),'DisplayName','Scanning from speaker 1 (SOI)',...
        'LineStyle','-','LineWidth',2,'Color','black'); %
    plot(phiVec/pi*180,10*log10(bp(2,:)),'DisplayName','Scanning from speaker 2 (Int.)',...
        'LineStyle','-','LineWidth',2,'Color',[1 1 1]/2); %
    set(gca,'fontsize',12);xlabel(['azimuth angle \phi (view from \theta=',num2str(theta_scan*180/pi),')'])
    ylabel('Gain, dB.');
    if farfieldbp==1
        title(['Far Field Beampattern ref:',num2str(array_ref),' '])
    else
        title(['Near Field Beampattern ref:',num2str(array_ref)])
    end
    plot([phi_SOI,phi_SOI],(10*log10([0.001,1])),'DisplayName','SOI','LineWidth',2,'Color','black'); %Plot the SOI DOA
    plot([phi_Int,phi_Int],(10*log10([0.001,1])),'DisplayName','Int','LineStyle','--','LineWidth',2,'Color','red') %Plot the Int DOA
    axis([-1   361   -80     5])
    grid on
else %if plotdB
    if farfieldbp==1
        figure('Name',['Far Field Beampattern']);
    else
        figure('Name',['Near Field Beampattern']);
    end
    hold on
    plot(phiVec/pi*180,(bp(1,:)),'DisplayName','Scanning from speaker 1 (SOI)',...
        'LineStyle','-','LineWidth',2,'Color','black'); %
    plot(phiVec/pi*180,(bp(2,:)),'DisplayName','Scanning from speaker 2 (Int.)',...
        'LineStyle','-','LineWidth',2,'Color',[1 1 1]/2); %
    set(gca,'fontsize',12);
    xlabel(['azimuth angle \phi, degrees'])
    ylabel('Gain');
    title(['Ring Array Beampattern ref:',num2str(array_ref)])
    plot([phi_SOI,phi_SOI],([0.001,1]),'DisplayName','SOI','LineWidth',2,'Color','black'); %Plot the SOI DOA
    plot([phi_Int,phi_Int],([0.001,1]),'DisplayName','Int','LineStyle','--','LineWidth',2,'Color','red') %Plot the Int DOA
    axis([phiVec(1)*180/pi-1 phiVec(end)*180/pi+1 0 1]);
    grid on
end %if plotdB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Perform beamforming with the data signal

%Length of signal to process
siglength = size(array_sig,2);
if 0 %So it is faster inside a loop
    maxsiglngth = max(intindexes.noiseend,intindexes.intend)+250;
    siglength = maxsiglngth; %Override so we do not expend too much time in the loop
end

% Perform overlapping windows
frame_len=FFT_len;
overlap=frame_len/2; %Overlap at 50%!!
total_num=fix(siglength/(frame_len-overlap)); %Number of frames with overlap

%Hamming window
ham_win=ones(N_array_els,1)*hamming(frame_len)';

out1=zeros(1,total_num*(frame_len-overlap));%vector store beamformer output.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Process the samples with the weight vector for each frequency bin
for i_frame=1:total_num-1 %Loop for all frames
    %short frame processing
    cur_data=zeros(N_array_els,FFT_len);%matrix store current frame data
    framestart_circ=(i_frame-1)*(frame_len-overlap)+1;
    frameend_circ  =(i_frame-1)*(frame_len-overlap)+frame_len;

    cur_data(:,1:frame_len)=ham_win.*array_sig(:,framestart_circ:frameend_circ);

    %Frequency data of current frame, row:elements, col:freq_bins
    F_cur_data=(fft(cur_data.',FFT_len)).';%note, in Matlab FFT is done to the column vector.

    F_out1=zeros(1,FFT_len);

    %Perform narrow-band beamforming at each frequency bin
    for m=2:FFT_len/2
        F_out1(m)=v1(:,m)'*F_cur_data(:,m); %DS beamformer
    end

    for m=1:FFT_len/2
        F_out1(FFT_len-m+1)=conj(F_out1(m+1));%make its IFFT real
    end

    temp1=ifft(F_out1,FFT_len);
    temp1=real(temp1);
    framestart_circ=(i_frame-1)*(frame_len-overlap)+1;
    out1(framestart_circ:frameend_circ)=out1(framestart_circ:frameend_circ)+temp1(1:frame_len);

end %for i_frame=1:total_num-1 %For all the frames do


%%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Performance measures of interference and noise output power from chunks of data
figure('Name',['Beamformer output signal']);
plot(out1,'DisplayName',...
    ['Output signal'],'Marker','none','LineStyle','-','LineWidth',2,'Color','blue');
set(gca,'fontsize',12);
title('Output signal')
legend(gca,'Location','SouthEast');
axis([0 length(out1) -1 1])
grid on

%From samples intindexes.noisestart to intindexes.noiseend we can collect the noise ouput power very good
%Edinburgh data. For dft data I need to create data files with only noise
out1noise = out1(intindexes.noisestart:intindexes.noiseend);%size(out1noise)
sigmanoise = 10*log10(out1noise*out1noise'/4000);
%disp(['10logPn : ',num2str(sigmanoise)]);

%From samples intindexes.intstart to intindexes.intend we can collect interference plus noise
out1intplusnoise = out1(intindexes.intstart:intindexes.intend);%size(out1)
sigmaintplusnoise = 10*log10(out1intplusnoise*out1intplusnoise'/4000);
%disp(['10logPin: ',num2str(sigmaintplusnoise)]);

%Write the output in a wave file so we can visualize it later in SF
if 1
    wavwrite(out1,Fs,32,'..\data\out\out1.wav');
end
%soundsc(out1,Fs); %play the sound so we can hear it

disp(['Ref:(',num2str(array_ref(1)),')10logPn : ',num2str(sigmanoise)]);
disp(['Ref:(',num2str(array_ref(1)),')10LogPin: ',num2str(sigmaintplusnoise)]);

disp('End  fBB.m')
return
%This is the return of the main function

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot the location of the microphones/sensors
function fmicposLocal(mpos,srcpos,array_ref)
figure('Name',['Simulation configuration']);
plot3(srcpos(1,1),srcpos(2,1),srcpos(3,1),'DisplayName',...
    ['SOI'],'Marker','square','LineStyle','none','LineWidth',2,'Color','blue');
hold on;
plot3(srcpos(1,2),srcpos(2,2),srcpos(3,1),'DisplayName',...
    ['Int.'],'Marker','square','LineStyle','none','LineWidth',2,'Color',[0 0 0.5]);
hold on;
plot3(array_ref(1),array_ref(2),array_ref(3),'DisplayName',...
    ['Reference'],'Marker','pentagram','LineStyle','none','LineWidth',2,'Color','black');

plot3(mpos(1,:),mpos(2,:),mpos(3,:),'DisplayName',...
    ['Microphones'],'Marker','o','LineStyle','none','LineWidth',2,'Color','red');
set(gca,'fontsize',12);
title('Simulation configuration')
%legend(gca,'Location','SouthEast');
legend(gca,'Location','best');
%axis([-1.5000         0   -1.0000    0.5000         0    1.5])
xlabel('x(m)')
ylabel('y(m)')
zlabel('z(m)')
%axis square;
grid on
return %function fmicposLocal(mpos,srcpos)

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Obtain the polar coordinates from the cartesian coordinates of a source
function [phi_out,theta_out,r_out]=fInlFindPhiSOIInt(i_speaker,xspeakref,yspeakref,zspeakref)
if i_speaker==1
    %Find SOI bearing with respect the reference
    xSOIref = xspeakref; %SOI x coordinate with respect the reference
    ySOIref = yspeakref; %SOI y coordinate with respect the reference
    zSOIref = zspeakref; %SOI z coordinate with respect the reference
    rSOI = sqrt(xSOIref^2+ySOIref^2+zSOIref^2); %SOI distance with respect the reference
    if (xSOIref>=0 && ySOIref>=0)
        phi_SOI = asin(ySOIref/rSOI)*180/pi;
    elseif (xSOIref<0 && ySOIref>=0)
        phi_SOI = acos(xSOIref/rSOI)*180/pi;
    elseif (xSOIref<0 && ySOIref<0)
        phi_SOI= atan(ySOIref/xSOIref)*180/pi+180;
    elseif (xSOIref>=0 && ySOIref<0)
        phi_SOI= atan(ySOIref/xSOIref)*180/pi+360;
    end
    theta_SOI = acos(zSOIref/rSOI)*180/pi;
    phi_out = phi_SOI;
    theta_out = theta_SOI;
    r_out = rSOI;
elseif i_speaker==2
    %Find Int bearing with respect the reference
    xIntref = xspeakref; %Int. x coordinate with respect the reference
    yIntref = yspeakref; %Int. y coordinate with respect the reference
    zIntref = zspeakref; %Int. y coordinate with respect the reference
    rInt = sqrt(xIntref^2+yIntref^2+zIntref^2); %Int. distance with respect the reference
    %phi_Int = atan(yIntref/xIntref)*180/pi+360; %We add 360 because yIntref is negative!!!
    if (xIntref>=0 && yIntref>=0)
        phi_Int = asin(yIntref/rInt)*180/pi;
    elseif (xIntref<0 && yIntref>=0)
        phi_Int = acos(xIntref/rInt)*180/pi;
    elseif (xIntref<0 && yIntref<0)
        phi_Int= atan(yIntref/xIntref)*180/pi+180;
    elseif (xIntref>=0 && yIntref<0)
        phi_Int= atan(yIntref/xIntref)*180/pi+360;
    else
        disp('Error, ???');keyboard;
    end

    theta_Int = acos(zIntref/rInt)*180/pi;
    phi_out = phi_Int;
    theta_out = theta_Int;
    r_out = rInt;

else
    disp('Error, >2 speakers');keyboard;
end
return %function [phi_SOI,phi_Int,r_out]=fInlFindPhiSOIInt(i_speaker,xspeakref,yspeakref,zspeakref)


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%-mpos: microphones positions
%-srcpos: sources positions. can be anywhere in 3D.
%-array_ref: reference where amplitude is one and time delay is zero
%-intindexes: indexes where the Pn and Pin is obtained from
function [array_sig,file_Fs,sa_delay]=fInlgenerateSimSignal(mpos,srcpos,array_ref,desiredspeaker)

N_speakers = 2;
for i_speaker = 1:N_speakers %For each speaker
    xspeaker = srcpos(1,i_speaker); %Absolute x coordinate of the source
    yspeaker = srcpos(2,i_speaker); %Absolute y coordinate of the source
    zspeaker = srcpos(3,i_speaker); %Absolute y coordinate of the source
    % Distance of the speakers to the array center
    xspeakref = xspeaker-array_ref(1);
    yspeakref = yspeaker-array_ref(2);
    zspeakref = zspeaker-array_ref(3);

    %Find the phi_SOI and phi_Int for this particular scenario
    %%%%%%%
    if i_speaker==1
        [phi_SOI,theta_SOI,rSOI]=fInlFindPhiSOIInt(i_speaker,xspeakref,yspeakref,zspeakref);
    elseif i_speaker==2
        [phi_Int,theta_Int,rInt]=fInlFindPhiSOIInt(i_speaker,xspeakref,yspeakref,zspeakref);
    end
end

%Find the DOA of SOI and interference
disp('Begin generating received array signal...');
DOA=[phi_SOI  theta_SOI; phi_Int  theta_Int;]; %az, pol; az, pol; Polar is always 90 because we are in the xy plane
DOA=DOA';%After transpose, desired signal DOA is in 1st column,interference DOAs in following columns
DOA=DOA/180*pi;%convert to radian

%Read the SOI from a wavefile
if desiredspeaker==1
    [SOI,file_Fs]=wavread('..\data\speech_dft.wav'); %SOI from wave files
else
    [SOI,file_Fs]=wavread('..\data\LoopyMusic.wav'); %SOI from wave files
end
maxlength = 95800;
SOI=SOI(1:maxlength); %This is a column vector
SIR = -1; %Not used so far
SNR = -1; %Not used so far
use_default=0;
disp_flag=1;
%Create the array signal. How to do that? Use Y. Li code
[array_sig,sa_delay]=signal_con(DOA,SIR,SNR,SOI,rSOI,rInt,mpos,use_default,disp_flag,array_ref,desiredspeaker);
return %[array_sig,file_Fs,sa_delay]=fInlgenerateSimSignal(mpos,srcpos,array_ref,desiredspeaker))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%view(135,30), axis on, grid on
%%xlabel('samples'), ylabel('amplitude')
