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

function []=fplotNFBeamp(pIn)


   %Get the input parameters
   current_freq=pIn.current_freq; %Frequency of the signal
   v_0 =pIn.v_0;
   c=pIn.c;
   mpos=pIn.mpos;
   array_ref=pIn.array_ref;
   srcpos=pIn.srcpos;
   mindist=pIn.mindist;

   gainpower = 1;
   N_array_els=size(mpos,2);

   %Plot the beampattern from the weights vector
   phiresolution = 2*pi/100;
   phiVec2D=0:phiresolution :2*pi; %scanning azimuth angle

   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
   

   thetaSrc = acos(zscantoref/r_scan)*180/pi; %Scan at the elevation of the selected speaker
   phiSrc   = acos(xscantoref/(r_scan*sin(thetaSrc)))*180/pi;
%keyboard
   thetaVec2D=(0)*pi/2/90:pi/64:(180)*pi/2/90; %elevation angles to scan
   itotal = 0; %Total index
   figure('Name',['bp 2-D cuts']); %Because the figure is inside the loop
   for ithetaVec=1:length(thetaVec2D),
      theta_scan=thetaVec2D(ithetaVec);
      for i_phiVec=1:length(phiVec2D), %Loop for the azimuth angle
         itotal = itotal +1;
         phi_scan=phiVec2D(i_phiVec);
         if round(theta_scan*180/pi)==round(thetaSrc) %Stop at SOI DOA
             %keyboard
             if round(phi_scan*180/pi)==round(phiSrc) %Stop at SOI DOA
                 %keyboard
             end
         end
         %Find the scanning direction, cosines DOA scanning
         x_dcos=sin(theta_scan)*cos(phi_scan);
         y_dcos=sin(theta_scan)*sin(phi_scan);
         z_dcos=cos(theta_scan);

         %Find the cartesian coordinates of the scanning when center is zero!!
         x_scan=r_scan*x_dcos;
         y_scan=r_scan*y_dcos;
         z_scan=r_scan*z_dcos;
         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: ',num2str(mindist)]);  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;
         bp1(i_phiVec,ithetaVec)=abs(s_scan_nearfield'*v_0)^2;

         %The beampattern is the gain at a DOA set by the direction cosines
         xcart(itotal)= (bp1(i_phiVec,ithetaVec))*x_dcos;
         ycart(itotal)= (bp1(i_phiVec,ithetaVec))*y_dcos;
         zcart(itotal)= (bp1(i_phiVec,ithetaVec))*z_dcos;
      end; %for i_phiVec=1:length(phiVec2D),
      plot(phiVec2D/pi*180,(bp1(:,ithetaVec))/0.1120,'DisplayName',['Delay-and-Sum'],'LineStyle','--',...
         'LineWidth',2,'Color',0.8*[1 1 1]/ithetaVec^(1/3));
      hold on
   end;  %for ithetaVec=1:length(thetaVec2D),
   %keyboard
set(gca,'fontsize',12);
xlabel(['azimuth angle \phi (view from \theta=[',num2str(thetaVec2D(1)*180/pi),...
    ', ',num2str(thetaVec2D(end)*180/pi),'])'])
ylabel('Gain');
title([num2str(N_array_els),'-Array 2D Bp'])

   
   %% Plot RA it in a linear mesh
   [thetamesh,phimesh]=meshgrid(thetaVec2D,phiVec2D);
   meshRAbp = figure('Name',['3D map'],'Color',[1 1 1]);
   axes('Visible','off','Parent',meshRAbp,'XDir','reverse','YGrid','on','XGrid','on');
   %axes('Parent',meshRAbp,'XDir','reverse','FontSize',12);
   surf(thetamesh*180/pi,phimesh*180/pi,10*log10(bp1/0.1120))
   view([65.5 10]);
   set(gca,'fontsize',12);xlabel('Polar angle {\theta}');
   ylabel('Azimuth angle {\phi}'); zlabel('Magnitude');
   title([num2str(N_array_els),'-Array 3D Bp unwrapped']);
   
   %% Create a RA 3-D beampattern with surf
   %Create the mesh
   [thetamesh,phimesh]=meshgrid(thetaVec2D,phiVec2D);
   [xcartm,ycartm,zcartm]=sph2cart(phimesh,pi/2-thetamesh,bp1/0.1120);
   fig3Dmesh = figure('Name',['3D bp mesh'],'Color',[1 1 1]);
   % Create axes
   axes('Visible','on','Parent',fig3Dmesh,'YGrid','off','YDir','reverse','XGrid','off',...
      'XDir','reverse');
   %xlim([-1 1]); ylim([-1 1]); zlim([-1 1]); 
   view([-35.5 24]);
   hold('all');
   % Create xlabel
   set(gca,'fontsize',12);xlabel(['x, coordinate'])
   ylabel('y, coordinate');zlabel('z, coordinate')
   surf(xcartm,ycartm,zcartm);


   %Create our own axes
   hold on
   plot3([0 2],[0 0],[0 0],'MarkerSize',14,'Color',[0 0 1])
   plot3([0 0],[0 2],[0 0],'MarkerSize',14,'Color',[1 0 0])
   plot3([0 0],[0 0],[0 2],'MarkerSize',14,'Color',[0 0 0])
   % Create textbox
   annotation(fig3Dmesh,'textbox','String',{'y'},'FitHeightToText','off',...
      'EdgeColor',[1 1 1],'Color',[1 0 0],'FontSize',12,...
      'Position',[0.8703 0.258 0.02692 0.04097]);

   % Create textbox
   annotation(fig3Dmesh,'textbox','String',{'z'},'FitHeightToText','off',...
      'EdgeColor',[1 1 1],'FontSize',12,...
      'Position',[0.5375 0.9087 0.02692 0.04097]);

   % Create textbox
   annotation(fig3Dmesh,'textbox','String',{'x'},'FitHeightToText','off',...
      'EdgeColor',[1 1 1],'FontSize',12,'Color',[0 0 1],...
      'Position',[0.1057 0.3545 0.02692 0.04097]);

   title([num2str(N_array_els),'-SA 3D Bp']);
   grid on
   
return