clear all
close all

sbdFolder = '\\atlas.shore.mbari.org\ProjectLibrary\CPF\Operations\2021Nov16\';
fileList = ls([sbdFolder, '\*.sbd']);

for i=1:size(fileList,1)
    fprintf("Processing SBD number %d: %s\n", i, fileList(i,:))
    fix(i).sbdString = fileread([sbdFolder,'\',fileList(i,:)]);
    fprintf("Msg: %s\n", fix(i).sbdString)
    sbdFields = split(fix(i).sbdString, ',');
    
    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(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
    
    tempFields = split(sbdFields(29), ' ');
    fix(i).profileNum = tempFields(1);
    
    fix(i).timestampString = sbdFields{1};
    fix(i).timestampNum = datenum(sbdFields{1},'yyyy-mm-ddTHH:MM:SS');
    
    fix(i).battV = str2double(sbdFields(33));
    
end
unsortedFixTable = struct2table(fix);
fixTable = sortrows(unsortedFixTable, 'timestampNum');
fprintf(1,'\nSBD attachments sorted by time\n')
fprintf(1,'Msg: %s\n', string([fixTable.sbdString]))

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

figure
plot([fixTable.timestampNum], fixTable.battV);
datetick('x');
title('Battery Voltage vs. Time');

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

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

chartFig = figure('Name','GPS Fixes (easting, northing)','NumberTitle','off', 'Units', 'normalized', 'Position', [0.1 0.1 0.4 0.7]);
grid on
%[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;
grid on
hold on

for i=2:height(fixTable)
    fixTable.range(i) = sqrt( (fixTable.easting(i) - fixTable.easting(i-1))^2 + (fixTable.northing(i) - fixTable.northing(i-1))^2 );
    angle  = atan2d((fixTable.northing(i) - fixTable.northing(i-1)), (fixTable.easting(i) - fixTable.easting(i-1)));
    fixTable.heading(i) = mod(90 - angle, 360);
    fixTable.speed(i) = fixTable.range(i) / ( (fixTable.timestampNum(i) - fixTable.timestampNum(i-1)) * 24.0);
    fixTable.predictedEasting(i) = fixTable.easting(i) + (fixTable.range(i) * cosd(angle));
    fixTable.predictedNorthing(i) = fixTable.northing(i) + (fixTable.range(i) * sind(angle));
    [fixTable.predictedLat(i), fixTable.predictedLon(i)] = minvtran(utm.struct, fixTable.predictedEasting(i), fixTable.predictedNorthing(i));
end

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

futureHours = 12;
futureEasting = fixTable.easting(end) + (fixTable.speed(end) * futureHours * cosd(angle));
futureNorthing = fixTable.northing(end) + (fixTable.speed(end) * futureHours * sind(angle));
plot(futureEasting, futureNorthing, 'wX')

lastIndex = height(fixTable);
fixTable.easting(lastIndex+1) = fixTable.easting(lastIndex) + (fixTable.speed(lastIndex) * futureHours * cosd(angle));
fixTable.northing(lastIndex+1) = fixTable.northing(lastIndex) + (fixTable.speed(lastIndex) * futureHours * sind(angle));
fixTable.profileNum(lastIndex+1) = fixTable.profileNum(lastIndex);
fixTable.timestampString(lastIndex+1) = {datestr( (fixTable.timestampNum(lastIndex) + 0.5), 'yyyy-mm-ddTHH:MM:SSZ')};
[fixTable.latDD(lastIndex+1), fixTable.lonDD(lastIndex+1)] = minvtran(utm.struct, fixTable.easting(lastIndex+1), fixTable.northing(lastIndex+1));

plot(fixTable.easting(end), fixTable.northing(end), 'ro');

RSB = sprintf('\nLast fix to this fix range: %4.1f m  speed: %4.0f m/hour  compass heading: %3.1f deg', fixTable.range(end), fixTable.speed(end), fixTable.heading(end));
title(RSB)
fprintf(1, '%s\n', RSB);

writeCPFTrack(fixTable, sbdFolder);

s = serialport("COM8",9600,"Timeout",5);
fixNum = 1;
for i=1:100
    gpsLine = readline(s);
    if contains(gpsLine, 'GGA')
        ggaFields = split(gpsLine, ',');
        latString = ggaFields(3,:);
        latDeg = latString{1}(1:2);
        latMin = latString{1}(3:end);
        gpsFix(fixNum).latDD = str2double(latDeg) + (str2double(latMin) / 60.0);
        if(ggaFields(4,:) == "S")
            gpsFix(fixNum).latDD = -1.0 * gpsFix(fixNum).latDD;
        end
        
        lonString = ggaFields(5,:);
        tempFields = split(lonString, '.');
        lonDeg = tempFields{1}(1:end-2);
        lonMin = [tempFields{1}(end-1:end), '.', tempFields{2}];
        gpsFix(fixNum).lonDD = str2double(lonDeg) + (str2double(lonMin) / 60.0);
        if(ggaFields(6) == "W")
            gpsFix(fixNum).lonDD = -1.0 * gpsFix(fixNum).lonDD;
        end
        
           
    gpsFix(i).timestampString = ggaFields{1};
    fix(i).timestampNum = datenum(sbdFields{1},'yyyy-mm-ddTHH:MM:SS');
        
        fprintf(1, 'fixNum: %d  Lat: %s   Lon: %s\r', fixNum, gpsFix(fixNum).latDD, gpsFix(fixNum).lonDD);
        
        fixNum = fixNum + 1;
    end
end
clear s
