clear; close all;
tpii = 'tp2.txt';
gps = 'gps2.txt';
beacon = 1;

% ---READ TRACKPOINT II FILE FOR VEHICLE SLANT RANGE

fid = fopen(tpii);
i=1;

% Read file one line at a time...
while feof(fid) < 1,
   a = fgets(fid);
   
   b = rot90(sscanf(a, '%f %f:%f:%f %f %f %f %f %f %f'));
   
   % Only use lines with full input 
   if length(b) == 10,
      b1(i, 1:10) = b;
      i = i+1;
   end
end
fclose(fid);

% Only use lines for correct beacon & no errors
ok = find((b1(:,1) == beacon) & (b1(:,10) == 0));
b1 = b1(ok,:);

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

figure; 
subplot(2,1,1); 
title(tpii);
plot(tp_time, tp_sr);
xlabel('Time of Day (seconds)');
ylabel('Slant Range(m)')
subplot(2,1,2); 
plot(tp_time, tp_br);
xlabel('Time of Day (seconds)');
ylabel('Bearing(deg)')

% ---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
      gps_time(i) = (sscanf(a(8:9), '%f')-7)*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:35),'%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; 
plot(gps_long2, gps_lat2);
title(gps);
xlabel('Latitude');
ylabel('Longitude');
figure; 
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 = 2.5;
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

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

[x, y] =  mfwdtran(tp_lat, tp_long);

% ---FIND BEST ESTIMATE OF POSITION ... USE 1487 m/s FOR SOS...
sos = 1500;
tat = 0.015;
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));
[lat_auv long_auv] = minvtran(best(1), best(2));

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);

%-----
return

best1500 = fmins('cum_range_error', position, 0, [], x, y, tp_sr);
best1490 = fmins('cum_range_error', position, 0, [], x, y, tp_sr*1490/1500);
best1487 = fmins('cum_range_error', position, 0, [], x, y, tp_sr*1487/1500);
best1480 = fmins('cum_range_error', position, 0, [], x, y, tp_sr*1480/1500);
best1470 = fmins('cum_range_error', position, 0, [], x, y, tp_sr*1470/1500);
[lat_auv1500 long_auv1500] = minvtran(best1500(1), best1500(2));
[lat_auv1490 long_auv1490] = minvtran(best1490(1), best1490(2));
[lat_auv1487 long_auv1487] = minvtran(best1487(1), best1487(2));
[lat_auv1480 long_auv1480] = minvtran(best1480(1), best1480(2));
[lat_auv1470 long_auv1470] = minvtran(best1470(1), best1470(2));

figure;
best = best1500;
sr_err = tp_sr'-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;
title('1500');

figure;
best = best1490;
sr_err = 1490/1500*tp_sr'-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;
title('1490');

figure;
best = best1487;
sr_err = 1487/1500*tp_sr'-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;
title('1487');

figure;
best = best1480;
sr_err = 1480/1500*tp_sr'-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;
title('1480');

figure;
best = best1470;
sr_err = 1470/1500*tp_sr'-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;
title('1470');

f = 1;best = fmins('cum_range_error', position, 0, [], x, y, tp_sr*f);
cum_range_error(best, x, y, tp_sr*f)
f = 1488/1500;best = fmins('cum_range_error', position, 0, [], x, y, tp_sr*f);
cum_range_error(best, x, y, tp_sr*f)
f = 1487/1500;best = fmins('cum_range_error', position, 0, [], x, y, tp_sr*f);
cum_range_error(best, x, y, tp_sr*f)
f = 1486/1500;best = fmins('cum_range_error', position, 0, [], x, y, tp_sr*f);
cum_range_error(best, x, y, tp_sr*f)