clear all
close all

sbdFolder = 'C:\Users\magene\Documents\CPF\Operations\2021Nov16';
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) < 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');
         battV(i) = str2double(sbdFields(14));

    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');
         battV(i) = str2double(sbdFields(31));
    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

figure
plot([fix.timestampNum], 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);

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

chartFig = figure('Name','GPS Fixes (easting, northing)','NumberTitle','off');
[mbay, R] = readgeoraster('U:\MapsChartsEtc\montbay_colorshd.tif');
mapshow(mbay,R);
xLimits = xlim;
yLimits = ylim;

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

numFixesToPlot = 30;
maxNumFixes = 1000;
for i=1:maxNumFixes
    ship(i).easting = 0;
    ship(i).northing = 0;
    ship(i).latDD = 0;
    ship(i).lonDD = 0;
    ship(i).tof = 0;
end

sp = serialport("COM6", 460800);

fixNum = 0;
xlim = xLimits;
ylim = yLimits;

while(true)
    fixNum = min(maxNumFixes, fixNum + 1);
    ship(2:maxNumFixes) = ship(1:maxNumFixes-1);
    ship(1).latDD = 0;
    ship(1).lonDD = 0;
    ship(1).easting = 0;
    ship(1).northing = 0;
    ship(1).tof = 0;
   
    msg = readline(sp);
    while(msg{1}(1:6) ~= "$GNGGA")
        msg = readline(sp);
    end
 
    ggaFields = split(msg, ',');
    latString = ggaFields(3);
    latDeg = latString{1}(1:2);
    latMin = latString{1}(3:end);
    ship(1).latDD = str2double(latDeg) + (str2double(latMin) / 60.0);
    if(ggaFields(4) == 'S')
        ship(1).latDD = -1.0 * ship(1).latDD;
    end
    
    lonString = ggaFields(5);
    tempFields = split(lonString, '.');
    lonDeg = tempFields{1}(1:end-2);
    lonMin = [tempFields{1}(end-1:end), '.', tempFields{2}];
    ship(1).lonDD = str2double(lonDeg) + (str2double(lonMin) / 60.0);
    if(ggaFields(6) == 'W')
        ship(1).lonDD = -1.0 * ship(1).lonDD;
    end

    [ship(1).easting, ship(1).northing] = mfwdtran(utm.struct, ship(1).latDD, ship(1).lonDD);
    plot(ship(1).easting, ship(1).northing, 'rO');
    if(fixNum >= 2)
        plot(ship(1:2).easting, ship(1:2).northing);
    end
    xlim = xLimits;
    ylim = yLimits;
    
    fprintf(1, 'GGA message: %s', msg);
    fprintf(1, 'Ship TOF: %10.7f\n', ship(1).tof);
    fprintf(1, 'Ship Lat: %10.7f\n', ship(1).latDD);
    fprintf(1, 'Ship Lon: %10.7f\n\n', ship(1).lonDD);
end