% This MatLab script applies a mathematical model to underwater
% recordings of whale calls.  It is steered by a list of times of the
% calls, and extracts a pressure time series from a data archive.
%   1.  Recursive calculation of the call centroid.
%   2.  band-pass filtering
%   3.  linear least-squares estimation of call parameters (frequency,
%      chirp rate, time and phase).
% Nov 18 2002 Michael Hoffman, SFSU
% modified January 21 2003
% exit gates modified January 22
% 2005 version, started Feb 11. 2005
% Revision with "central frequency" as a parameter - RWB, March 13, 2005

% Data from a "bwc" scan file are assumed to have been loaded into
% memory.  The Matlab function which does this is bwcload.m .
% format of bwc files
%     * npeaks,cfile,iyear,cday,cdb,ccen,csig,cthrnsig,cthrfix
%     * npeaks (long): number of peaks
%     * cfile[npeaks] (string(50)): "c" file name, including path
%     * iyear[npeaks] (int): year number (2001 -> 1, etc)
%     * cday[npeaks] (double): starting times (ms)
%     * cdb[npeaks] (float): peak amplitude, in dB re (???)
%     * ccen[npeaks] (float): center of noise peak for 2-hr run
%     * csig[npeaks] (float): sigma of noise peak for 2-hr run
%     * cthnsig[npeaks] (float): threshold at "n sigmas"
%     * cthfx[npeaks] (float): fixed threshold
% For manual scans (like the Baby B's), the last five fields are all set
% equal to 1.

%%%%%%%%%%%%%%%%%%%%%%%% Constants and Parameters %%%%%%%%%%%%%%%
tt=cputime;

I=find(cdb>0); % Select the sequence of events to process.

N=length(I);
x=[];
LB=[];
UB=[];
OPTIONS=optimset('lsqnonlin');
OPTIONS.Display='off';
data=[];

% New parameters - RWB, 3-13-2005
fzero = 16.; % fundamental frequency, assumed value
alfzero = 0.045; % frequency droop rate (Hz/sec)
alflow=-.045;
alfhigh=0.135;
harm = 3.; % harmonic number to be fit.
fsearch = fzero*harm; % frequency to search at
dfhw = 0.1; % fraction of central frequency (half width) to define band.
fslow=fsearch*(1.-dfhw); % lower limit on f to search
fshigh=fsearch*(1.+dfhw); % upper limit on f to search
alfsrch = alfzero*harm; % value of droop rate
alfslow = alflow*harm; % lower limit on alpha to search
alfshigh = alfhigh*harm; % upper limit on alpha to search
fcheat = 48.58; % Might as well really cheat.
alfcheat = 0.18;
fbestguess=16.2; % Best guess for signal frequency, for signal and noise
                 % calculations.
dfhwsn=0.0625; % HW for determination of signal to noise.
                 
fsamp = 1000.; % sampling frequency
nmaxfit = 3001; % Maximum number of points to fit.
eps1 = 1.; % epsilon to keep squared-signal sums from vanishing
frontrange1=7000;
backrange1=7000;
frontrange=15000;
backrange=15000;
ncycav=5; % Number of cycles to average over to get the envelope.
nptav=floor(ncycav/fsearch*fsamp); % Number of points to average for envelope.

% Determine the lengths of the time series to be used in progression
% of least-squares fits.  Suppose that fsamp is the sampling frequency,
% and that the signal lies in a passband [fsearch +/- fsearch*dfhw].
% If we associate the signal with one value of the FFT frequency comb,
% and do the same for the test signal that we start the fit with, we
% can require that they always associate with the same frequency.  I 
% think that this is tantamount to requiring that the test signal always
% line up with the actual signal, provided that it is within the pass
% band.
%  This requires that
%    deltaf-FFT = 2 x (full width of pass band).
% Since the duration T of the test signal is given by
%    T = 1/(deltaf-FFT) = nsamp-max * sampling period,
% the maximum duration in terms of number of samples which will give a
% lock onto any signal in the pass band is
%    n0 = nsamp-max = f-samp / (4 * fsearch * dfhw).
% A more conservative argument might lead to n0 twice this large.  However,
% today (March 13, 2005) I'll start with this algorithm.

n0 = fsamp/(4.*fsearch*dfhw); % starting number of points to fit
gates = [n0/2, n0, 2*n0, 5*n0, 10*n0, 20*n0, 40*n0, 100*n0, ...
        200*n0, 500*n0]; % sequence of gates
% Truncate the sequence 
igate=1;
while gates(igate) < nmaxfit/2, igate=igate+1; end
ngates=igate;
gates(ngates) = nmaxfit/2;
gates=floor(gates(1:ngates)); % The final sequence of gates.
% For fsamp = 1000 Hz, fsearch = 48 Hz, and dfhw = 0.05, 
% gates = [52, 104, 208, 521, 1042, 1500] 
fprintf(1,'%8.0f  ',gates);

more off
iplot=0; %plot control

%%%% Begin loop over scanned.  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
start=1;
for n=start:N
% for n=522:557
    x=[fsearch,alfsrch,0]; %seed for fit [f0, alpha(droop rate), central phase]

    index=I(n);
    fprintf(1,'n: %8.0f; index: %8.0f\n',n,index);
    
    eventday=cday(index);
    
    file=cfile(index,1:47);
    file(47)='b'; %modifying cfile name to respective bfile path/name
    file(27)='2'; %july 15 2003
    fprintf(1,'b file: %s\n',file);
    
    fid=fopen(file,'r');
    
    day0=fread(fid,1,'double');
    npt=fread(fid,1,'long');
    twohrs=fread(fid,npt,'int16'); % time series, about 2 hrs long
    fclose(fid);
    
    eventelement=eventday-day0+1; % index at call position

    %*** Define range of indices for signal, protecting against under-
    %*** and over-run of array.
    callrange=eventelement-backrange:eventelement+frontrange-1;
    if eventelement+frontrange>npt+1 
        callrange=eventelement-backrange+1:npt;
    end
    if eventelement<backrange
        callrange=1:eventelement+frontrange;
    end
     
    call=twohrs(callrange); % call is the time series to analyze.
   
    %figure(2)
    %plot(call);
    % figure(2)
    % specgram(call,1024,1000,kaiser(512,2),256);
    % hold off
   
    fcall=fundamentR(call,fsamp,fslow,fshigh);
    L=length(fcall);
    E=zeros(1,L); % initializing column vector for envelope
   
    % If eflag = 1, calculate an envelope for fcall.  If eflag = 0,
    % use the envelope E supplied. Then find the power-weighted centroid
    % centrd1 of the envelope.
    % If iplot >= 1, plot signal and envelope.
    eflag=1; % Value to force calculation of the envelope.
    [E1,cntrd1]=envelopeR(fcall,nptav,eflag,E,iplot);
   
    % Using a narrower time window, loop to recursively find the
    % envelope center.
    for k=1:5
       
       if cntrd1<1
           cntrd1=round(mean([1 L]));
       end
       callrange1=cntrd1-backrange1:cntrd1+frontrange1-1;

       if (cntrd1+frontrange1)>L+1
            callrange1=cntrd1-backrange1+1:L;
       end
       if cntrd1<backrange1
            callrange1=1:cntrd1+frontrange1;
       end        
   
       fcall1=fcall(callrange1); % recentered time series
       E=E1(callrange1); % envelope corresponding to fcall1
       [E,cntrd]=envelopeR(fcall1,nptav,0,E,iplot); % Find call center and plot.
  
       cntrd1=cntrd+cntrd1-backrange1; %cntrd w/respect to original long piece
    end
    % This is the new, centered, filtered call time series.
    fcall=fcall1;
    icen=callrange(1)+cntrd1; % index wrt 2-hour series
    eday=day0+icen; % year time in msec
%%%%%%%%%%%%%%%%%%%%%%%%%%%% end cntrd %%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%% estimate phi0 %%%%%%%%%%%
    L=length(fcall);
    omega=x(1)*2*pi/1000;
    tsig=[1:L]-cntrd;
    bsig=fcall;
    % Reduce this range, for the third harmonic.  RWB, 3-7-2005
    % I1=cntrd-300;
    % I2=cntrd+300;
    I1=cntrd-100;
    I2=cntrd+100;
   
    t=tsig(I1:I2)';
    b=bsig(I1:I2);
    c=sum(b.*cos(omega.*t));
    s=sum(b.*sin(omega.*t));
    c1=sum(cos(omega.*t).^2);
    c2=sum(cos(omega.*t).*sin(omega.*t));
    s1=c2;
    s2=sum(sin(omega.*t).^2);
   
    d=s2*c1-c2*s1;
    cosphi0=(s2*c-c2*s)/d;
    sinphi0=(s1*c-c1*s)/d;
    phi0gs=atan2(sinphi0,cosphi0);
   
    x(3)=phi0gs;
    fprintf(1,'initial guess:   [%8.4f, %8.5f, %8.4f]\n',x);
    %%%%%%%%%%%% end phi0guess%%%%%%%%%%
   
   %%%%%%%%fittting with successive gates%%%%%%%%   
   exits=0; % One success bit for each of last 5 fits.
   for igate=1:ngates
       gate=floor(gates(igate));

       [x,res,resid,exitflag]=lsqnonlin(@bfit2pi,x,LB,UB,OPTIONS,fcall,E,cntrd,gate);
       plotcfit2pi(x,fcall,E,cntrd,gate);
       title(['day=' num2str(eventday/86400000) ', f0=' num2str(x(1)) ' alpha=' num2str(x(2)) ' exits=' num2str(exits)]) 
       if exitflag<1 exitflag=0; end
       if iplot >= 2
         fprintf(1,'igate: %5i; x: [%8.4f, %8.5f, %8.4f]\n', ...
         igate,x); end
       % The frequency and slope are constrained to stay in bounds.
       x(1)=max([fslow,min([fshigh,x(1)])]);
       x(2)=max([alfslow,min([alfshigh,x(2)])]);       
       if iplot >= 2 fprintf(1,'igate: %5i; x: [%8.4f, %8.5f, %8.4f]\n', ...
         igate,x); end
       % Set a status bit for each of the last 5 fits.
       ibit=5-ngates+igate;
       if ibit >= 1 exits=exits+exitflag*2^(ibit-1); end
   end
%    'waiting...'
% pause
   % cfit2pi returns the fitted function X1, the envelope, and the data
   % that was fit.  From these we calculate chi2, a normalized "badness"
   % of fit.  It is equal to the sum of the squared residuals, divided
   % by the product of the norms of fitted function and data.  RWB.
   [X1,jnk,b1]=cfit2pi(x,fcall,E,cntrd,gates(ngates));
   X1absq=X1'*X1;
   b1absq=b1'*b1;
   resabsq=sum((X1-b1).^2);
   %%%% Mod to protect against case of signals all zero.  RWB, 3-7-2005
   chi2=10; % value for exceptions
   if X1absq*b1absq > 0
       %%%% Changing the way this is calculated.  RWB, 3/7/2005
       % This shouldn't change anything  -  it's just  that the operator
       % near the end should be just * , not .* .
       %%%% chi2=sum((X1-b1).^2)/sqrt((X1'*X1).*(b1'*b1));
       chi2=resabsq/sqrt(X1absq*b1absq);
   end    
   if iplot >= 2 fprintf(1, ...
     'progressive;  x: [%8.4f, %8.5f, %8.4f]; chi2: %8.4f\n',x,chi2); end

   % fitting with the largest gate and best guess ("cheating fit") 
   cheatguess=[fcheat,alfcheat,phi0gs];
   [chx,chres]=lsqnonlin(@bfit2pi,cheatguess,LB,UB,OPTIONS,fcall,E,cntrd,gates(ngates));

   % Recalculate the "chi-squared," to see if it improved.
   [Xch,jnk,bch]=cfit2pi(chx,fcall,E,cntrd,gates(ngates));
   % chi2_ch=sum((Xch-bch).^2)/sqrt((Xch'*Xch).*(bch'*bch));% normalized "badness" of fit
   X1absq=Xch'*Xch;
   b1absq=bch'*bch;
   resabsq=sum((Xch-bch).^2);
   chi2_ch=10; % value for exceptions
   if X1absq*b1absq > 0
       chi2_ch=resabsq/sqrt(X1absq*b1absq);
   end    
   if iplot >= 2 fprintf(1, ...
     'cheating;     x: [%8.4f, %8.5f, %8.4f]; chi2: %8.4f\n',chx,chi2_ch); end
  
   if chi2-chi2_ch>.02
       fprintf(1,'Cheating fit is better.  chi2: %7.4f; chi2_ch: %7.4f\n', ...
         chi2,chi2_ch);
       x=chx;
       res=chres;
       exits=exits+2^5;
       chi2=chi2_ch;
       plotcfit2pi(x,fcall,E,cntrd,gates(ngates));
       title(['day=' num2str(eventday/86400000) ', f0=' num2str(x(1)) ' alpha=' num2str(x(2)) ' exits=' num2str(exits)]) 
   end
   
   % Calculate signal and signal-to-noise for five harmonics.
   b=call(callrange1); % shortened call, final centering
   sigdb=zeros(1,5);
   [sigdb,s2ndb]=signoise(b,fbestguess,dfhwsn,sigdb,iplot);
   
   
   %%%%%%%%%% concatenation of data array %%%%%%%%
%      data=[data; [eday,x,res,chi2,exits,index]];
      data=[data; [eday,x,res,chi2,exits,index,sigdb,s2ndb]];
      
   fprintf(1,'N%4i; n%4i; x: [%8.4f, %8.5f, %8.4f]; exits: %3i\n', ...
     N,n,x,exits);
end%end of fitting loop

timebfitexec=cputime-tt % execution time in seconds

% After this program finishes, write the data file with fwritebfitexecR2.m:

% fwritebfitexecR2  Write routine for bfitexecR2 (baby B's, 3rd harmonic)
% The file format has to be changed, so I will add a data which will function
% as a version number.
% The data in the "data" array are [eday,x[3],res,chi2,exits,index,
% sigdb(5),s2ndb(5)],
% written in log order.  They will be written as separated time series.
% RWB, March 28, 2005
% fout='/Users/guest/seamount/project/0502/baby/bbscan/bbscan_mar2805_3rdharmonic.fit';
% version=50328;
% [npt,datacol]=size(data)
	
% fid=fopen(fout,'w')
% fwrite(fid,[version,npt],'long'); % version number, followed by number of points
	
% fwrite(fid,data(:,7),'long'); % index number of event in input file
% fwrite(fid,data(:,8),'long'); % fit status flag
	
% for d=1:6 fwrite(fid,data(:,d),'double'); end % eday, x[3], res, chi2
% for d=9:datacol fwrite(fid,data(:,d),'double'); end % sigdb and s2ndb for 5 harms.
	
% fclose(fid)