%Author: B. Kieft - based on the inital work of Jim Bellingham's Trackpoint II localization code. History prior to that unknown.
%
%Note: This code assumes certain values. Set appropriately for your use:
%SeafloorDepth - default 300 m
%UTM zone - default 10s 
%
%It was also originally written to make use of a Garmin 16x GPS unit and a generation 4 Universal Deck Box from 
%Benthos. See below for details on the deck box script to acquire range in the correct format. Adjust parsing
%code as needed to accomodate different equipment. Bearing could easily be added as well if the tracking system
%supports it. 
%
%Copyright (c) 2012,1013 MBARI
%MBARI Proprietary Information.  All Rights Reserved
%

close all;


% Generate warning
warning(['fromDegrees is located in maputils. Latest was at /mbari/matlab/toolbox/shared/ ',...
    'optimset is located in /mbari/matlab/toolbox/matlab/optimfun/\n',...
    ])

addpath ./include
addpath ./data

if( exist('fileNumber') )
  fileEnd = strcat(fileNumber,'.txt');
  tpii = strcat('range',fileEnd);
  gps = strcat('gps',fileEnd);
else
  tpii = 'rangeMerged.txt';
  gps = 'gpsMerged.txt';
end

if ~exist(tpii, 'file'),
  tpii_original = tpii;
  tpii = fullfile('QA_data/', tpii);
  gps = fullfile('QA_data/', gps);
end

if ~exist(tpii, 'file'),
  error(['Can not find input file ' tpii_original]);
end

beacon = 1;

if( exist('SeafloorDepth') )
  seafloor = SeafloorDepth;
else
  seafloor = 300;
end

% ---Read range file for vehicle slant range

% -- How to get a range file and what it looks like -- 
%On generation 4 Universal Deck Boxes (UDB) there is filesystem support. This means one can write scripts to run commands on the modems or UDBs.
%Here is the script called "range" that is saved in /ffs/bin/range. Chmod the file to allow executable permissions.
%<<
%:loop
%date
%ATR$2
%goto loop
%<<
%E.g. type "range 1" to continuously range to address 1. The format of the range file looks like this:
%<<
%Wed Jan 30, 2013  12:56:05
%Range=250.5 Meters
%Wed Jan 30, 2013  12:56:09
%Range=255.5 Meters
%Wed Jan 30, 2013  12:56:13
%Range=248.7 Meters
%...
%<<
%Now let's open the file....


fid = fopen(tpii);
i=1;

% Read file one line at a time...
while feof(fid) < 1,
  a = fgetl(fid); % First, get the date line
  rangeLine = fgetl(fid); % Then the range
 % a = strcat(a, rangeLine) % And combine
  
  [b,count] = sscanf(strcat(a, rangeLine), '%*s %*s %*f, %*f  %f:%f:%fRange=%f');  
  b = rot90(b);
   
   % Only use lines with full input 
   if length(b) >= 3,
      b1(i, 1:length(b)) = b;
      i = i+1;
   end
end
fclose(fid);


% Pull out TOD and slant range
tp_time=b1(:,1)*3600+b1(:,2)*60+b1(:,3);
tp_sr = b1(:, 4);


% ---READ GPS FILE FOR POSTION
%
%This GPS file is generated using a Garmin 16x GPS unit. 
%It is configured to only output GPGGA strings. The frequency is 1 Hz.
%Here is a sample from the file:
%<<
%$GPGGA,125601,3648.1340,N,12147.2953,W,1,08,1.4,8.1,M,-33.2,M,,*71
%$GPGGA,125602,3648.1340,N,12147.2953,W,1,08,1.4,8.1,M,-33.2,M,,*71
%$GPGGA,125603,3648.1340,N,12147.2953,W,1,08,1.4,8.1,M,-33.2,M,,*71
%<<

fid = fopen(gps);
i=1;

% Read file one line at a time...
while feof(fid) < 1,
   a = fgets(fid);
   if length(a) > 35 & strcmp(a(1:7),'$GPGGA,')==1, % Note - DON'T convert time to PDT
     gps_time(i) = (sscanf(a(8:9), '%f'))*3600+sscanf(a(10:11), '%f')*60+sscanf(a(12:13), '%f');
     gps_lat(i) = sscanf(a(15:16),'%f') + sscanf(a(17:23),'%f')/60;
     gps_long(i) = -(sscanf(a(27:29),'%f') + sscanf(a(30:36),'%f')/60);
   i = i+1;   
   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_lat2 = gps_lat2./gps_n2;
gps_long2 = gps_long2./gps_n2;

%figure out timezone adjustment

tz_adj = round(( mean(tp_time)-mean(gps_time))/3600)
tp_time = tp_time - tz_adj * 3600;

figure('Name', 'Slant Range'); 
title(tpii);
plot(tp_time, tp_sr);
xlabel('Time of Day (seconds)');
ylabel('Slant Range(m)')

figure('Name', 'Lat/Lon'); 
plot(gps_long2, gps_lat2);
title(gps);
xlabel('Latitude');
ylabel('Longitude');

figure('Name', 'Lat/Lon vs. Time'); 
subplot(2,1,1); 
plot(gps_time2, gps_lat2);
title(gps);
xlabel('Time of Day (seconds)');
ylabel('Latitude(deg)');
subplot(2,1,2); 
plot(gps_time2, gps_long2);
xlabel('Time of Day (seconds)');
ylabel('Longitude(deg)');

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

delay = 1.71; % Delay for software on deck box.
tp_lat = interp1q(gps_time2', gps_lat2', tp_time+delay)';
tp_long = interp1q(gps_time2', gps_long2', tp_time+delay)';

% ---CONVERT LAT,LONG to UTM
% UTM zone 10S is selected here. Could make this user settable at some point when using outside of this UTM zone. 
if exist('axesm'),
    axesm utm 
    setm(gca,'zone','10s')
%    axesmui % Do we really need a GUI? nah... Change zone above if necessary
%     setm(gca,'zone',ans)
    [x, y] =  mfwdtran(tp_lat, tp_long);
end

% ---FIND BEST ESTIMATE OF POSITION ... USE 1500 m/s FOR speed of sound in sea water if user doesn't define it as something else.
if( exist('SOS') )
  sos = SOS;
else
  sos = 1500;
end
if( exist('TAT') )
  tat = TAT;
else
  tat = 0.0;
end

tp_src = (tp_sr - (0.001 - tat)*750)*sos/1500;

ok = find(~isnan(x) & ~isnan(y));
position = [mean(x(ok)) mean(y(ok)) 761 1500];
best = fmins('cum_range_error', position, 0, [], x(ok), y(ok), tp_src(ok));

if exist('minvtran')
  [lat_BED long_BED] = minvtran(best(1), best(2)) 
%else
%  [lat_BED long_BED] = utm2deg(best(1), best(2), '10 S')
end


figure;
sr_err = tp_src'-sqrt((x - best(1)).^2 + (y - best(2)).^2 + best(3)^2);
neg = find(sr_err < 0); stem3(x(neg),y(neg),sr_err(neg), 'r'); hold on;
pos = find(~(sr_err < 0)); stem3(x(pos),y(pos),sr_err(pos), 'b'); hold off;
title1 = sprintf('%4d %2.1f ', sos, delay); title(title1);

% Run JGB code to plot range circles
rangeCircles;

% Plot the range delta between hits
rangeDelta;

% Send output to UTM log to use for least squares fit in
% LBL survey code
[x,y,zone] =  deg2utm(tp_lat, tp_long);

fid = fopen(['BED_pings_bound.log'], 'wt');
for i=1:length(x)
  fprintf(fid, '%f,%f,%f\n', x(i), y(i), tp_sr(i));
end
fclose(fid)
    
    x = x';
    y = y';
%% Used as input to LBL survey code
%-----
return

