%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Compute a beacon location from the ship's GPS data and
%          the sonar log produced by the Benthos deck unit (DS-7000?).
% AUTHOR:  Jim Bellingham
% DATE:    Circa July 1999
%
% MODS:    00/8/27 RSM: Small bug fixes and a little polishing here
%          and there...
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clear; close all;
%
%  *** Data Entry Section ***
%
% ----Determine which beacons will be calibrated....
beacon = 4;
lbl = sprintf('ds7k-L%1.0f.txt',beacon);
gps = sprintf('gps-L%1.0f.txt',beacon);
%
% Enter the lat/long at which each beacon was dropped from the ship, for a sanity
% check.
  Drew = [(36 + 56.456/60)   -(122 + 7.900/60) ;
          (36 + 55.000/60)   -(122 + 7.410/60) ;
          0.                 0.                ;      % Not deployed
          (36 + 53.505/60)   -(122 + 6.965/60) ;
          (36 + 55.012/60)   -(122 + 9.781/60) ;
         ];
%
% Channel is the deck-unit's channel number.  The column is the
% beacon number.
%
channel = [1 1 1 1 1];

% ----Parameters important for synchronizing & mapping time-of-flight to range

delay = [40 40 40 40 40]; % Mis-synchronization of gps and lbl (in seconds)
sos = 1460;  % Speed of sound (m/s)  sos = 1492 typical....
tat = [3 10 3 3 3]/1000; % Turn-around-time of transponders:usually 0.003 seconds
%
% Latitude and longitude of the array, truncated to integer degrees.
%
  lat = 36;
  lon = -122;
%
  figPos = [4 32 815 658];
%
% *** End Data Entry Section ***
%
% ---READ DS-7000 FILE FOR LBL BEACON SLANT RANGE

fid = fopen(lbl);
i=1;

% Read file one line at a time...
tt_min = 50/750; % minimum travel time in seconds
ii = 1;
b1 = zeros(ii,4);
b = fgets(fid);

while feof(fid) < 1,

   if length(b) > 3,
      while ~strcmp(b(1:3),'@PR'), % If a transmit, then get the time....
         b = fgets(fid);
         if b == -1, break, end;
         if length(b)<3, break, end;
      end;
   end;
   if b == -1, break, end;

   c = sscanf(b,'@PR=%f N %f/%f/%f %f:%f:%f')';
   b1(ii,:) = [c(5:7) 99];

   b = fgets(fid);
   if b == -1, break, end;

   while (~strcmp(b(1:3),'@PR')), % Look for first return....
      c = sscanf(b, '@%f %f %f %f');
      if length(c) == 4,
         if ((c(1)==channel(beacon)) & (c(4)>tt_min) & (c(4)< 1)),
            b1(ii,4) = c(4);
            count = 1;
            b = fgets(fid);
            break;
         end;
      end;
      b = fgets(fid);
      if b == -1, break, end;
   end;
   if b == -1, break, end;

   ii=ii+1;
end;
fclose(fid);

% Pull out TOD and travel time
ok = find(b1(:,4) < 10); % Cull out pings with no returns...
lbl_time=b1(ok,1)*3600+b1(ok,2)*60+b1(ok,3);
lbl_tt = b1(ok, 4);

% ---Do a mediam filter on travel times...

medtt = medfilt1(lbl_tt, 3);
ok = find(abs(lbl_tt - medtt) < 0.01);
lbl_tt = lbl_tt(ok);
lbl_time = lbl_time(ok);

% ---PLOT LBL DATA!
figure(1);
set(gcf,'position',figPos)
clf
title(lbl);
plot(lbl_time, lbl_tt, '-x');
xlabel('Time of Day (seconds)');
ylabel('Travel Time (s)')
zoom on
grid on
hold on

% ---READ GPS FILE FOR POSTION

fid = fopen(gps);
i=1;

% Read file one line at a time...
while feof(fid) < 1,
   a = fgets(fid);
   if (a(1:7) == '$GPGGA,' & length(a) > 35) % Note - convert time to PDT
      b = sscanf(a, '$GPGGA,%f,%f,N,%f');
      if (length(b) == 3 & length(a)>=30)
         hr = floor(b(1)/10000);
         min = floor((b(1) - hr*10000)/100);
         sec = b(1) - hr*10000- min*100;
         gps_time(i) = (((hr-7)*60)+min)*60+sec; % Time in PDT
         gps_lat(i) = (b(2)-3600)/60 + 36;
         gps_long(i) = -((b(3) + lon*100)/60 - lon);
         al(i) = length(a);
         i = i+1;
      end;
   end
end
fclose(fid);

% for gps update within the same second...average lats and longs....

j = 1;
gps_n2(j) = 1;
gps_time2(j) = gps_time(1);
gps_lat2(j) = gps_lat(1);
gps_long2(j) = gps_long(1);
for i = 1:length(gps_time),
   if abs(gps_time(i) - gps_time2(j)) < 0.05,
      gps_lat2(j) = gps_lat2(j) + gps_lat(i);
      gps_long2(j) = gps_long2(j) + gps_long(i);
      gps_n2(j) = gps_n2(j) + 1;
   else
      j = j+1;
      gps_time2(j) = gps_time(i);
      gps_lat2(j) = gps_lat(i);
      gps_long2(j) = gps_long(i);
      gps_n2(j) = 1;
   end;
end;
gps_lat = gps_lat2./gps_n2;
gps_long = gps_long2./gps_n2;
gps_time = gps_time2;


figure(2);
set(gcf,'position',figPos)
clf
zoom on
grid on

subplot(2,1,1);
plot(gps_time, gps_lat);
title(gps);
xlabel('Time of Day (seconds)');
ylabel('Latitude(deg)');
zoom on
grid on

subplot(2,1,2);
plot(gps_time, gps_long);
xlabel('Time of Day (seconds)');
ylabel('Longitude(deg)');
zoom on 
grid on

% ---CHECK LBL TIME
% Make sure that we only work with LBL data which are bracketed by GPS positions

ok = find((lbl_time > gps_time(1)) & (lbl_time < gps_time(length(gps_time))));
lbl_time = lbl_time(ok);
lbl_tt = lbl_tt(ok);

% ---CONVERT LAT,LONG to UTM

figure(14);
axesm utm;
axesm; % This will cause a gui to pop up...choose zone 10S

[gps_east, gps_north] =  mfwdtran(gps_lat, gps_long);
[loc_e  loc_n]        =  mfwdtran(Drew(:,1), Drew(:,2));
DrewUTM = [loc_e loc_n];

% ---INTERPOLATE GPS POSTIONS TO LAT LONG TIME BASE

x = interp1q(gps_time', gps_east', lbl_time+delay(beacon))';
y = interp1q(gps_time', gps_north', lbl_time+delay(beacon))';

if beacon == 1
   %Hand Edit.  Remove bad data.  The bad tof's appear to be echos.  This was observed
   %in the error plot in the first pass through.
   goodIndx = [ ...   
     1     2     3     4     5     6     7     8     9    10    11    12 ...
    13    14    15    16    17    18    19    20    21    22    23    24 ...
    25    26    27    28    29    30    31    32    33    34    35    36 ...
    37    38    39    40    41    42    43    44    45    46    47    48 ...
    49    50    51    52    53    54    55    56    57    58    59    60 ...
    61    62    63    64    65    66    67    68    75    76    77    78 ...
    79    80    81    82    83    84    85    86    87    88    89    90 ...
    91    96    97    98    99   100   101   102   103   104   105   106 ...
   107   108   109   110   111   112   119   120   121   122   123   124 ...
   125   126   127   128   129   130   131   132   133   134   135   136 ...
   137   138   139   147   148   149   150   151   152   153   154   155];
	x = x(goodIndx);
   y = y(goodIndx);
   lbl_tt = lbl_tt(goodIndx);
end

% ---FIND BEST ESTIMATE OF POSITION ... ESTIMATE SOS ALSO...
lbl_src = (lbl_tt - tat(beacon))/2*sos;
ok = find(~isnan(x) & ~isnan(y));
position = [mean(x(ok)) mean(y(ok)) 100];
best = fmins('cum_range_error', position, 0, [], x(ok), y(ok), lbl_src(ok));

figure(3);
set(gcf,'position',figPos)
clf
if length(best) == 4,
   sr_err = best(4)/1500*lbl_src'-sqrt((x - best(1)).^2 + (y - best(2)).^2 + best(3)^2);
else
   sr_err = lbl_src'-sqrt((x - best(1)).^2 + (y - best(2)).^2 + best(3)^2);
end;%
neg = find(sr_err < 0); stem3(x(neg),y(neg),sr_err(neg), 'b'); hold on;
pos = find(~(sr_err < 0)); stem3(x(pos),y(pos),sr_err(pos), 'r'); hold off;
title1 = sprintf('%4d %2.1f ', sos, delay); title(title1);


figure(14);
[lat_auv long_auv] = minvtran(best(1), best(2));
close(14);

% Go back and color the lbl tot's also.  This can be illustrative.

figure(1)
line(lbl_time(pos), lbl_tt(pos), 'linestyle','x','color','r');

% Plot GPS ship position

north0 = gps_north(1);
east0  = gps_east(1);

figure(4);
set(gcf,'position',figPos)
clf
%plot(gps_long, gps_lat);
plot(gps_east - east0, gps_north - north0 )
line( best(1)-east0, best(2)-north0, 'linewidth', 2, 'linestyle','x','Color','m' );
line( DrewUTM(beacon,1)-east0, DrewUTM(beacon,2)-north0, 'linewidth', 2, 'linestyle','x','Color','g' );
title(gps);
%xlabel('Latitude');
%ylabel('Longitude');
xlabel( 'Relative UTM East' )
ylabel( 'Relative UTM North')
zoom on
grid on
set( gca, 'DataAspectRatio', [1 1 .25] );

%diary(['Beacon',num2str(beacon),'.txt'])
fprintf(' Transponder %2d freq %4.2f kHz:   %d Deg %7.4f Min  %d Deg %7.4f Min\n', ...
        beacon, c(2), lat, 60*(lat_auv-36), lon, -60*(long_auv - lon));
fprintf('      %9.1f East    %9.1f North   depth %5.1f\n',...
        best(1), best(2), best(3));
fprintf(' Mean error = %f meters.  Standard deviation = %f meters.\n', ...
        mean(sr_err), std(sr_err));

fprintf('Error from the drop location = %f meters.\n', norm(DrewUTM(beacon,:) - best(1:2)));
diary off


%-----
return