% This routine is writen to deal with the apparent altitude errors generated by
% the 150 kHz ADCP/DVL which are due to an incorrect scaling of range (i.e. show
% up in vehicle depth.
%
% Takes 'gain' as an input.
% Returns 'prod'.
%
% J. G. Bellingham

% Filter low frequency component out of depth using FFT
ok = find(~isnan(d));  
gt5 = find(d(ok) > 5);  % Make sure we don't use decent
n = length(ok);
ok = ok(gt5(1):n);

fftdepth = fft(d(ok)); % Do fft 
fd = d;
n = length(fftdepth);
nzero = 2;
fftdepth(1:(1+nzero)) = 0; % Zero LF
fftdepth((n-nzero+1):n) = 0; % Zero rest of LF
fd(ok) = real(ifft(fftdepth));
clear fftdepth; 

% Filter low frequency component out of altimeter using FFT
ok = find(~isnan(data(:, m_altitude)));  
gt5 = find(d(ok) > 5);  % Make sure we don't use decent
n = length(ok);
ok = ok(gt5(1):n);

fftalt = fft(data(ok, m_altitude)); % Do fft 
fa = data(:, m_altitude);
n = length(fftalt);
fftalt(1:(1+nzero)) = 0; % Zero LF
fftalt((n-nzero+1):n) = 0; % Zero rest of LF
fa(ok) = real(ifft(fftalt));
clear fftalt; 

% Select subset of the data we are going to use.
ok = find(isnan(fd) + isnan(fa) == 0);
gt5 = find(d(ok) > 5);
n = length(ok);
ok = ok(gt5(1):n);

gain = 0.5:0.05:1.5;
for i = 1:length(gain),
   % Find the mean of both depth and altitude.
   md = mean(mean(fd(ok)));
   mwd = mean(mean(gain(i)*fa(ok)+fd(ok)));
   
   % Compute the coupling product...
   prod = (fd(ok) - md) .* (gain(i)*fa(ok)+fd(ok) - mwd);
   
   cp(i) = sum(prod); 
end;

opt_gain = interp1(cp, gain, 0)

% Make a plot....
wd_opt = opt_gain* data(:, m_altitude) + d;
wd = data(:, m_altitude) + d;

plot(fd(ok), 'r');
hold on;
plot(wd_opt, 'g');
plot(wd, '.g');
plot((fd(ok) - md) .* (opt_gain*fa(ok)+fd(ok) - mwd), 'b');
mwd = mean(mean(1*fa(ok)+fd(ok)));
plot((fd(ok) - md) .* (1*fa(ok)+fd(ok) - mwd), '.b');
hold off;
zoom on;

% clear ok; clear gt5; clear n; clear md; clear mwd;
clear wd; clear wd_opt