clear all
close all

sbdFolder = 'C:\Users\magene\Documents\CPF\Operations\2021Oct21';
fileList = ls([sbdFolder, '\*.sbd*'])

for i=1:size(fileList,1)
    fprintf("Processing SBD number %d: %s\n", i, fileList(i,:))
    sbdString = fileread([sbdFolder,'\',fileList(i,:)]);
    sbdFields = split(sbdString, ',');
    if( size(sbdFields,1) < 20)
         fullField = sbdFields(4);
         latFields = split(fullField);
         latString = latFields(1,:);
         latDeg = latString{1}(1:2);
         latMin = latString{1}(3:end);
         fix(i).latDD = str2double(latDeg) + (str2double(latMin) / 60.0);
         if(latFields{2}(1) == 'S')
             fix(i).latDD = -1.0 * fix(i).latDD;
         end
         
         fullField = sbdFields(5);
         lonFields = split(fullField);
         lonString = lonFields(1,:);
         tempFields = split(lonString, '.');
         lonDeg = tempFields{1}(1:end-2);
         lonMin = [tempFields{1}(end-1:end), '.', tempFields{2}];
         fix(i).lonDD = str2double(lonDeg) + (str2double(lonMin) / 60.0);
         if(lonFields{2}(1) == 'W')
             fix(i).lonDD = -1.0 * fix(i).lonDD;
         end
         fix(i).timestampNum = datenum(sbdFields{1},'yyyy-mm-ddTHH:MM:SS');

    else
         fullField = sbdFields(5);
         latFields = split(fullField);
         latString = latFields(1,:);
         latDeg = latString{1}(1:2);
         latMin = latString{1}(3:end);
         fix(i).latDD = str2double(latDeg) + (str2double(latMin) / 60.0);
         if(latFields{2}(1) == 'S')
             fix(i).latDD = -1.0 * fix(i).latDD;
         end
         
         fullField = sbdFields(7);
         lonFields = split(fullField);
         lonString = lonFields(1,:);
         tempFields = split(lonString, '.');
         lonDeg = tempFields{1}(1:end-2);
         lonMin = [tempFields{1}(end-1:end), '.', tempFields{2}];
         fix(i).lonDD = str2double(lonDeg) + (str2double(lonMin) / 60.0);
         if(lonFields{2}(1) == 'W')
             fix(i).lonDD = -1.0 * fix(i).lonDD;
         end
         fix(i).timestampNum = datenum(sbdFields{1},'yyyy-mm-ddTHH:MM:SS');
    end
end
unsortedFixTable = struct2table(fix);
fixTable = sortrows(unsortedFixTable, 'timestampNum');
fix.lonDD;
fix.lonDD;

%%
% figure('Name','GPS Fixes (lat, lon)','NumberTitle','off');
% hold on
% 
% plot(lonDD, latDD, '-+')
% daspect([1 1 1]);
% pbaspect([1 1 1]);
% grid
% hold off

utmstruct = defaultm('utm');
utmstruct.zone = utmzone(fixTable.latDD(1), fixTable.lonDD(1));
wgs84 = wgs84Ellipsoid;
ellipsoid = [wgs84.SemimajorAxis wgs84.Eccentricity];
utmstruct.geoid = ellipsoid;
utm.struct = defaultm(utmstruct);

[easting, northing] = mfwdtran(utm.struct, fixTable.latDD, fixTable.lonDD);
[latCheck, lonCheck] = minvtran(utm.struct, easting, northing);

figure('Name','GPS Fixes (easting, northing)','NumberTitle','off', 'Units', 'normalized', 'Position', [0.05, 0.1, 0.75, 0.75]);
[mbay, R] = readgeoraster('U:\MapsChartsEtc\montbay_colorshd.tif');
mapshow(mbay,R);
hold on
plot(easting, northing, 'r-+')

for i=2:length(fix)
    range = sqrt( (easting(i) - easting(i-1))^2 + (northing(i) - northing(i-1))^2 );
    bearing  = atan2((northing(i) - northing(i-1)), (easting(i) - easting(i-1)));
    
    predictedEasting(i) = easting(i) + (range * cos(bearing));
    predictedNorthing(i) = northing(i) + (range * sin(bearing));
    [predictedLat(i), predictedLon(i)] = minvtran(utm.struct, predictedEasting(i), predictedNorthing(i));
end

plot(predictedEasting(2:end), predictedNorthing(2:end), 'gX')