% This m file is an extension of ls3.m which calculates a least squares estimate fo the position of a 
% target based on ranges from the target to a grid of receivers at known locations (control points).  
% This m file also caculates a least squares estimate of the sound speed.  It is very important to note
% that this calculation is much more sensitive to geometry than that for
% position alone.  CP should be taken over a range of ranges including
% right over the presumed target position in order to keep the matrices
% well conditioned
% 

% Reference: "Analysis and Adjustment of Survey Measurements"
%            Edward M. Mikhail, Gordon Gracie
%            Van Nostrand Reinhold Company (out of print)
%            reprint acquired from E. M. Mikhail 
%            Purdue Univ, Civil Engr. Dept.

% variables: 
% CP = actual control point location (receiver locations)
% trueP = true position of the target
% trueRange = actual range between each receiver and the target
% range = measured range between each receiver and the target
% trueC = actual speed of sound (m/sec)\
% C = estimated speed of sound (m/sec)
% measuredTime = measured time of flight
% trueT = actual time of flight
% p = position estimate + speed of sound estimate
% W = weighting matrix (set to all ones for this example)
% B = coefficient matrix for the observations
% f = condition equations constant term vector
% N = coefficient matrix of the normal equations
% t = vector of "constants" in the normal equations
% delta = vector of parameter corrections
% v = residuals
% trueC = true sound speed velocity
% numRows = number of CPs to add right over the top of the target
% rangeEst = new estimate of the slant range
% travelTimeEst = new estimate of the one way travel time

% inputs: receiver locations,
%         range between target and each receiver
%         initial guess at target location
%         bias in range measurement
%         noise in range measurement
%         ship attitude - heading, pitch, roll

% outputs: delta (the residuals)
%          new position at each iteration

% Author: Gene Massion

% Revision History:    
% 12/13/04  initial release
%

clear all
close all

%dataFolder = 'C:\Users\gene\Documents\CPF\CPF003\Data\ParagonTests\2021Nov18-19\ParagonNanoGPSLocalization\';
dataFolder = '\\atlas\ProjectLibrary\CPF\CPF003\Data\ParagonTests\2021Nov18-19\CPF003Survey2021Nov19\';
gpsTrack = readlines([dataFolder 'GPSLog002.txt']);
rangeData = readtable([dataFolder 'RangeAndCPData.txt']);

ggaIndexes = find(contains(gpsTrack(1:end,1), '$GPGGA'));
ggaLines = gpsTrack(ggaIndexes);
ggaFields = split(ggaLines,',');

timeChars = char(ggaFields(:,2));
for i=1:length(timeChars)
    gpsFix.ggaTime(i) = datetime(2021,11,19,str2double(timeChars(i,1:2)), str2double(timeChars(i,3:4)), str2double(timeChars(i,5:6)), str2double(timeChars(i,8:9)));
end

latChars = char(ggaFields(:,3));
latDecPt = cell2mat(strfind(ggaFields(:,3), '.'));
for i=1:length(latChars)
    gpsFix.ggaLatDD(i) = str2double(latChars(i,1:latDecPt-3)) + str2double(latChars(i,latDecPt-2:width(latChars)))/60.0;
    if (strcmp(ggaFields(i,4), "S"))
        gpsFix.ggaLatDD(i) = -1.0 * gpsFix.ggaLatDD(i);
    end
end

lonChars = char(ggaFields(:,5));
lonDecPt = cell2mat(strfind(ggaFields(:,5), '.'));
for i=1:length(lonChars)
    gpsFix.ggaLonDD(i) = str2double(lonChars(i,1:lonDecPt-3)) + str2double(lonChars(i,lonDecPt-2:width(lonChars)))/60.0;
    if (strcmp(ggaFields(i,6), "W"))
        gpsFix.ggaLonDD(i) = -1.0 * gpsFix.ggaLonDD(i);
    end
end

utmstruct = defaultm('utm');
utmstruct.zone = utmzone(gpsFix.ggaLatDD(1), gpsFix.ggaLonDD(1));
wgs84 = wgs84Ellipsoid;
ellipsoid = [wgs84.SemimajorAxis wgs84.Eccentricity];
utmstruct.geoid = ellipsoid;
utm.struct = defaultm(utmstruct);

[gpsEasting, gpsNorthing] = mfwdtran(utm.struct, gpsFix.ggaLatDD, gpsFix.ggaLonDD);
[latCheck, lonCheck] = minvtran(utm.struct, gpsEasting, gpsNorthing);

latDiff = gpsFix.ggaLatDD - latCheck;
longDiff = gpsFix.ggaLonDD - lonCheck; 

chartFig = figure('Name','GPS Fixes (easting, northing)','NumberTitle','off', 'Units', 'normalized', 'Position', [0.1 0.1 0.7 0.7]);
grid on
axis equal
markerSize = 16;
%[mbay, R] = readgeoraster('\\atlas.shore.mbari.org\ProjectLibrary\CPF\Operations\MapsChartsEtc\montbay_colorshd.tif');
[mbay, R] = readgeoraster('montbay_colorshd.tif');
mapshow(mbay,R);
xLimits = xlim;
yLimits = ylim;
hold on
plot(gpsEasting, gpsNorthing, 'b-');
% 
% gps.latDD = rangeData.CPLat;
% gps.lonDD = rangeData.CPLon;
% [gps.easting, gps.northing] = mfwdtran(utm.struct, gps.latDD, gps.lonDD);
% plot(gps.easting, gps.northing)

startIndex = 1;
CPData.names(startIndex) = string(rangeData.WP(startIndex));
CPData.latDD(startIndex) = rangeData.CPLat(startIndex);
CPData.lonDD(startIndex) = rangeData.CPLon(startIndex);
for i=1:4
    nameIndex = find( (rangeData.WP(startIndex:end) ~= CPData.names(i)));
    CPData.names(i+1) = string(rangeData.WP(nameIndex(1) + startIndex));
    CPData.latDD(i+1) = rangeData.CPLat(nameIndex(1) + startIndex);
    CPData.lonDD(i+1) = rangeData.CPLon(nameIndex(1) + startIndex);
    startIndex = nameIndex(1) + startIndex - 1;
end
[CPData.easting CPData.northing] = mfwdtran(utm.struct, CPData.latDD, CPData.lonDD);
plot(CPData.easting, CPData.northing, 'g+', 'MarkerSize', markerSize);
plot(CPData.easting(1), CPData.northing(1), 'gO', 'MarkerSize', markerSize);


[rangeData.CPEasting rangeData.CPNorthing] = mfwdtran(utm.struct, rangeData.CPLat, rangeData.CPLon);

% Weight matrix = 1.0;
d = ones(1, height(rangeData));
W = diag(d,0);

% guess first position p
measuredC = 1500;
trueP = [rangeData.CPEasting(end) rangeData.CPNorthing(end) -205 1500];
p = [rangeData.CPEasting(end) rangeData.CPNorthing(end) -205 measuredC];

CP = [rangeData.CPEasting rangeData.CPNorthing zeros(length(rangeData.CPEasting),1)];
measuredTime = rangeData.TravelTime / 1000000;  %convert from microseconds to seconds
measuredTime = measuredTime - 0.2;              %subtract turnaround time
measureTime = measuredTime / 2.0;                %convert to one way travel time
travelTimeEst = measuredTime;

iter = 1;
delta = 1000 * ones(1,length(trueP));
while max(abs(delta)) > 0.1
    %p(3) = trueP(3);
    %p(4) = 1500;
    B = [(CP(:,1) - p(1))./(travelTimeEst .* p(4)^2) (CP(:,2) - p(2))./(travelTimeEst .* p(4)^2) ...
        (CP(:,3) - p(3))./(travelTimeEst .* p(4)^2) travelTimeEst./p(4)];
    f = ((1/p(4)) .* sqrt((CP(:,1) - p(1)).^2 + (CP(:,2) - p(2)).^2 + (CP(:,3) - p(3)).^2)) - measuredTime;
    f = f';
    
    % the simple inversion
    %delta = B\f'
    
    % the complicated inversion
    N = B' * W * B;
    t = B' * W * f';
    delta = inv(N) * t;
    v = f - (B * delta)';
    
    p = p + delta';
    rangeEst = sqrt((CP(:,1) - p(1)).^2 + (CP(:,2) - p(2)).^2 +(CP(:,3) - p(3)).^2)';
    travelTimeEst = rangeEst / p(4);
    travelTimeEst = travelTimeEst';
    
%     fprintf(1,'iter= %2.0f cond of N = %6.0f  dx= %6.2f dy= %6.2f dz= %6.2f dC= %8.2f estimates= %8.2f %8.2f %8.2f %8.2f\n', ...
%         iter, cond(N), delta(1), delta(2), delta(3), delta(4), p(1), p(2), p(3), p(4))
    fprintf(1,'iter= %2.0f dx= %6.2f dy= %6.2f dz= %6.2f dC= %8.2f estimates= %8.2f %8.2f %8.2f %8.2f\n', ...
        iter, delta(1), delta(2), delta(3), delta(4), p(1), p(2), p(3), p(4))
    iter = iter + 1;
end

distFromCPF003DU = sqrt( (rangeData.CPEasting(1)-p(1))^2 + (rangeData.CPNorthing(1)-p(2))^2);

plot(p(1), p(2), 'rX', 'MarkerSize', markerSize);
[latEst, lonEst] = minvtran(utm.struct, p(1), p(2));

fprintf(1,'\nfinal UTM estimates: x = %.1f  y = %.1f  z = %8.1f  C =  %.1f\n', ...
    p(1), p(2), p(3), p(4));
fprintf(1,'final estimates: longitude = %.6f  latitude = %.6f\n', lonEst, latEst);

latDeg = fix(latEst);
latDecMin = 60.0 * abs(latEst - latDeg);
lonDeg = fix(lonEst);
lonDecMin = 60.0 * abs(lonEst - lonDeg);
fprintf(1,'final estimates: longitude = %.0f %.3f  latitude = %.0f %.3f\n', lonDeg, lonDecMin, latDeg, latDecMin);

fprintf(1,'x error = %8.6f   y error = %8.6f   z error = %8.6f   C error = %8.6f\n\n', ...
    trueP(1) - p(1), trueP(2) - p(2), trueP(3) - p(3), trueP(4) - p(4));
fprintf(1,'Distance from CPF003DU = %.1f meters\n', distFromCPF003DU);

title(sprintf("Estimated Longitude: %.6f   Estimated Latitude: %.6f", lonEst, latEst))
legend('Paragon track', 'Survey Waypoints', 'CPF003DU Waypoint', 'Estimated Position')

