function [obsgood, frmax, amp, pha] = findfreq(data, del_t)
%
% FINDFREQ estimates the frequency, amplitude and phase of a sine wave interference 
% (due to  multiple OBSs) in each burst of OBS data.  FINDFREQ finds the frequency 
% with most energy by performing an fft on successively shorter data records. 
% FINDFREQ also produces some statistics on the amplitudes and phases, so values can be 
% chosen for cleaning the entire record.
%
%[obsgood, frmax, amp, pha] = findfreq(data, del_t)
%  input data = raw data
%         del_t = sampling interval, s
%
%  output  obsgood = clean data
%           frmax = frequency of removed signal
%	   amp = amplitude of removd signal in counts
%	   pha = phase lag of removed signal

% Jessie Lacy, USGS
% October 17, 2001

n = length(data);
mn = mean(data);
datadem = data-mn;
%
% find frequency with max energy, and neighbor with next-highest
% energy, from fft of full data set.

F = fft(datadem);
N = length(F);
frs = [0:1/n:(n-1)/n];
nlow = 5;
max_en = max(abs(F(nlow:N/2)));
imax = find(abs(F(nlow:N/2))==max_en)+nlow-1;
 amp = 2*max_en/N;
  pha = atan2(imag(F(imax)), real(F(imax)));
if abs(F(imax-1))>abs(F(imax+1))
  next = -1;
  ist = imax-2;
else
  next = 1;
  ist = imax-1;
end
imax2 = imax+next;
ist
frmax = (imax-1)/n;
%
% lim1 and lim2 bracket the record lengths we'll use to improve
% resolution of frequency with max energy
%
lim2 = n;
lim1 = floor(n*ist/(ist+1));
int = -1;
if (lim2-lim1)>200
  int = -2;
  end
%int = round((lim2-lim1)/20);
rec_len = [lim2:int:lim1];
%rec_len = [lim2:-1:lim1];
n_iter = length(rec_len)
%
% plot
%
figure(1) 
clf
subplot (3,1,1)

plot (frs/del_t,abs(F),'.')
xmax = frs(imax)*2/del_t;
%xmax = 0.2
ymax = max_en*2;
axis ([0 xmax 0 ymax])
title ('Fourier components: raw data(blue), finding peak(red),clean data (green)')
xlabel ('Frequency, s^-1')
ylabel ('Amplitude')
hold on
%
% find frequency with maximum energy by decreasing record length iteratively
% from lim2 to lim1, running fft at each step and finding max.
%
for k = 1:n_iter
%  PP(k);
  nk = rec_len(k);
  datak = data(1:rec_len(k));
  mnk = mean(datak);
  datademk = datak - mnk;
  Ft =fft(datademk);
  maxk = max(abs(Ft(nlow:ceil(nk/2))));
  if maxk >max_en
    max_en = maxk;
  imaxk = find(abs(Ft(nlow:ceil(nk/2)))==maxk)+nlow-1;
  frmax = (imaxk-1)/rec_len(k);
  plot (frmax/del_t,max_en,'r.')
  rec_len(k)
  amp = 2*max_en/rec_len(k);
  pha = atan2(imag(Ft(imaxk)), real(Ft(imaxk)));
  end
end

%calculate interfering signal from the frequency with max energy,
%and corresponding phase and amplitude.
%
t = [1:1:n];
bad = amp*cos(2*pi*frmax*t + pha);
bad = bad';
%
%clean data by subtracting out interfering signal
%
obsgood = data-bad;
%
%plot some results
%
fftgd = fft(obsgood);
plot (frs/del_t,abs(fftgd),'g.')
%legend ('raw data','finding peak','clean data')

offset = max(bad - datadem);
imin = find((bad - datadem)==offset);
bad2 = bad - (bad(imin(1))-datadem(imin(1)))+ mn;
%figure 
subplot (3,1,2)
plot (t, data, t, bad2,'r')
title ('Raw data (blue) and identified interference(red)')
axis ([0 n 0 2000])
subplot (3,1,3)
plot (t, obsgood)
title ('Clean data')
axis ([0 n 0 2000])



