clear all
close all

sbdFolder = '\\atlas\ProjectLibrary\CPF\Operations\2022Feb23\';
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,:)]);
    fix(i).sbdString = sbdString;
    sbdFields = split(sbdString, ',');
    if( size(sbdFields,1) < 25)
         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('C:\Users\magene\Documents\CPF\Data\AnalysisScripts\montbay_colorshd.tif');
mapshow(mbay,R);
hold on
plot(easting, northing, 'r-+')
title(string(datetime(now, 'ConvertFrom', 'datenum')));

for i=2:length(fix)
    range(i) = sqrt( (easting(i) - easting(i-1))^2 + (northing(i) - northing(i-1))^2 );
    bearingRad(i)  = atan2((northing(i) - northing(i-1)), (easting(i) - easting(i-1)));
    bearingDeg(i) = bearingRad(i) * 180.0 / pi();
    heading(i) = mod((90-bearingDeg(i)), 360);
    speedMperHr(i) = range(i) / ((fix(i).timestampNum - fix(i-1).timestampNum) * 24);
    
    predictedEasting(i) = easting(i) + (range(i) * cos(bearingRad(i)));
    predictedNorthing(i) = northing(i) + (range(i) * sin(bearingRad(i)));
    [predictedLat(i), predictedLon(i)] = minvtran(utm.struct, predictedEasting(i), predictedNorthing(i));
    predictedLat(end)
    predictedLon(end)
end

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

numHours = 9.5;

predictedEasting0900 = easting(end) + ((numHours*speedMperHr(end)) * cos(bearingRad(end)));
predictedNorthing0900 = northing(end) + ((numHours*speedMperHr(end)) * sin(bearingRad(end)));
plot(predictedEasting0900, predictedNorthing0900, 'bX')
hold off

dt = datetime(fixTable.timestampNum, 'ConvertFrom', 'datenum');
figure
yyaxis left
plot(dt, speedMperHr)
ylim([0 1000]);
ylabel('Speed (meters per hour)');
yyaxis right
plot(dt, heading)
ylabel('Compass heading (deg)');
title(string(datetime(now, 'ConvertFrom', 'datenum')));
