function MCS=mcs_init(xyz,cfreq,N_lam_xc,Nlam_min,Nlam_max,iifig)
%
% Initializes index vectors and matrices for lambda-subaperture
% matched cross-spectral beamforming.
%
%
% COPYRIGHT (c) 2008
% 
% Applied Research Laboratories, The University of Texas at Austin
% (ARL:UT)
% All rights reserved.
% 
% This material may be reproduced by or for the U.S. Government
% only. This notice must appear in all copies of this file and its
% derivatives. Further distribution outside the U.S. Government is
% prohibited without the explicit written authorization from the
% Director of the Environmental Sciences Laboratory, ARL:UT.
% 
% THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
% EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO THE
% IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
% PURPOSE, OR NONINFRINGEMENT.
% 
% IN NO EVENT WILL ARL:UT BE LIABLE FOR ANY DAMAGES WHATSOEVER ARISING
% OUT OF THE USE OR INABILITY TO USE THIS SOFTWARE, EVEN IF ARL:UT HAS
% BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
[N_ph,d3]=size(xyz);
dist0=sqrt(sum(diff(xyz,1).^2,2));
iiuni=all(abs(dist0-dist0(1))./dist0(1)<.01);	% Flag for uniform arrays;
%
cum_dist0=[0; cumsum(dist0)];
aper=cum_dist0(N_ph);
%
ncf=length(cfreq); c=1500; lambda=c./cfreq;
nsub=zeros(ncf,1);
ii1=1:N_ph-2; ii2=ii1+1;
for jf=1:ncf,
% First determine which element indices (iiph) to use at this frequency;
% Discard elements whose spacing is less than Nlam_min apart:
	dist_lam_min=round(cum_dist0/(Nlam_min*lambda(jf)));
	iiok=find(dist_lam_min(2:N_ph-1) > dist_lam_min(1:N_ph-2));
	iiph=[1; 1+iiok; N_ph]; nph=length(iiph);
% If array is longer than Nlam_max wavelengths, prune it:
	if nph<N_ph,
		dist=sqrt(sum(diff(xyz(iiph,:),1).^2,2));
		cum_dist=[0; cumsum(dist)]; 
	else
		dist=dist0; cum_dist=cum_dist0;
	end
	aper=cum_dist(nph); 
	aper_max=Nlam_max*lambda(jf); iileft=0;
	while aper > aper_max,
% Take away phones from end with largest spacing (or alternate if uniform array):
		if iiuni, iileft=~iileft; else, iileft=dist(1)>dist(nph-1); end
		if iileft,
			iiph=iiph(2:nph); dist=dist(2:nph-1); 
			cum_dist=cum_dist(2:nph)-cum_dist(2); nph=nph-1;
		else
			iiph=iiph(1:nph-1); dist=dist(1:nph-2); 
			cum_dist=cum_dist(1:nph-1); nph=nph-1;
		end
		aper=cum_dist(nph);
	end		
%
% Compute cumulative apertures in integer units of lambda*N_lam_xc:
	cum_ap=round(cum_dist/(lambda(jf)*N_lam_xc));
% Divide array into nsub subapertures, consisting of phones
% iisub1(jsub):iisub2(jsub), for jsub=1:nsub;
	iijump=find(diff(cum_ap)>0);
	if length(iijump)<=2, 
		[dum,nq1]=min(abs(cum_dist-.25*aper));
		[dum,nq2]=min(abs(cum_dist-.50*aper));
		[dum,nq3]=min(abs(cum_dist-.75*aper));
		iijump=[nq1;nq2;nq3],
	end
	iisub2=[iijump ; nph];
	nsub=length(iisub2);
	iisub1=[1; iisub2(1:nsub-1)+1];
	nph_sub=iisub2 - iisub1 + 1;	% Number of phones in each subaperture
	nph_sub_max=max(nph_sub);		% Max number of phones in subapertures
%
% Make index for placing weighted elements into array bf_mat that can be 
% summed vertically to achieve the subaperture summations:
	idx_bf=zeros(nph,1);
	for jsub=1:nsub,
		ii=iisub1(jsub):iisub2(jsub);
		idx_bf(ii)=(jsub-1)*nph_sub_max + [1:nph_sub(jsub)];
	end
%% Example usage to sum weighted elements to get subarray beams:
%%	bf_mat=zeros(nph_sub_max,nsub);
%%	bf_mat(idx_bf)=bf;
%%	bf_sub=sum(bf_mat,1);
%
% Make indices of subapertures to crosscorrelate:
	if nsub>2, nskip=2; else, nskip=1; end
	n_xc=(nsub-nskip+1)*(nsub-nskip)/2;
	idx_xc1=zeros(n_xc,1); idx_xc2=idx_xc1; nxc=0;
	for jsub=1:nsub-nskip,
		iixc=[jsub+nskip:nsub]';
		nxc_cur=nsub - (jsub+nskip) + 1;
		idx_xc1(nxc+1:nxc+nxc_cur)=jsub;
		idx_xc2(nxc+1:nxc+nxc_cur)=iixc;
		nxc=nxc + nxc_cur;
	end
	if nxc~=n_xc, disp('nxc not equal to n_xc in mcs_init'); keyboard; end
%% Example usage to get correlate pairs of subarray beams and sum 
%% to get MCS beamformer output:
%%	bf_mcs=sum(bf_sub(idx_xc1).*bf_sub(idx_xc2));
%
% Save quantities in output structure MCS:
	MCS(jf).iiph=iiph;
	MCS(jf).nph=nph;
	MCS(jf).aper=aper;
	MCS(jf).nsub=nsub;
	MCS(jf).nph_sub_max=nph_sub_max;
	MCS(jf).iisub=[iisub1,iisub2];
	MCS(jf).bf_mat=zeros(nph_sub_max,nsub);
	MCS(jf).idx_bf=idx_bf;
	MCS(jf).n_xc=n_xc;
	MCS(jf).idx_xc1=idx_xc1;
	MCS(jf).idx_xc2=idx_xc2;
	MCS(jf).nsum=nsub*nph_sub_max + n_xc;
%int2str([iisub1,iisub2]),
end
% 
if iifig,
	colors='rbgmcyk'; ncol=length(colors);
	symbols='osdv<>^ph'; nsym=length(symbols);
	for jf=1:ncf,
		figure;
		iiph=MCS(jf).iiph;
		plot_cfill(cum_dist0(iiph),cum_dist0(iiph),'s','k',8); hold on; axis('ij');
		col=colors(mod(jf,ncol)+1);
		for jxc=1:MCS(jf).n_xc,
			col=colors(mod(jxc,ncol)+1);
			sym=symbols(mod(jxc,nsym)+1);
			jsub1=MCS(jf).idx_xc1(jxc);
			jsub2=MCS(jf).idx_xc2(jxc);
%			keyboard
			iisub1=iiph(MCS(jf).iisub(jsub1,1):MCS(jf).iisub(jsub1,2));
			iisub2=iiph(MCS(jf).iisub(jsub2,1):MCS(jf).iisub(jsub2,2));
			for jph1=1:length(iisub1),
				plot_cfill(cum_dist0(iisub1(jph1)),cum_dist0(iisub2),'o',col,5);
			end
		end
		axis([0,cum_dist0(N_ph),0,cum_dist0(N_ph)])
		pause(.001);
		keyboard
	end
end

