clear all
close all

% Created by Gene Massion
% 11/04/2021, Modified by Magdalena:
% - use dir as opposed to ls to list files. save figures. print lat,lon of
%  predicted position in Map
% 11/16/2021, modified by MC to adjust field columns (SBD essage has more
% info). Plotting Health Status Data

% Specify folder to read SBDS and save kml track and plots
sbdFolder = '2022Mar29\';
path_plots=sbdFolder;

% fileList = ls([sbdFolder, '*.sbd*']);
fileList = dir([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,:)]);
    
    display(["Processing SBD number %d: %s\n", i, fileList(i).name])
    fix(i).sbdString = fileread([fileList(i).folder,'/',fileList(i).name]);
    
    
    fprintf("Msg: %s\n", fix(i).sbdString)
    sbdFields = split(fix(i).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
         
         tempFields = split(sbdFields(27), ' ');
         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(14));

    else
         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(15), ' ');
         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));
         
         fix(i).canP = str2double(sbdFields(35)); % MC
         fix(i).canHum = str2double(sbdFields(37)); % MC
    end
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
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]);
[mbay, R] = readgeoraster('\\atlas\ProjectLibrary\CPF\Operations\MapsChartsEtc\montbay_colorshd.tif');
% [mbay, R] = readgeoraster('/Volumes/CPF/Operations/MapsChartsEtc/montbay_colorshd.tif');

mapshow(mbay,R);
xLimits = xlim;
yLimits = ylim;

hold on
plot(fixTable.easting, fixTable.northing, 'r-+')
plot(fixTable.easting(end), fixTable.northing(end), 'ro');

for i=2:length(fix)
    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')
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);

text(fixTable.predictedEasting(end)+0.01, fixTable.predictedNorthing(end)+0.01, ['(',latDeg,' ', latMin,', ',lonDeg,' ', lonMin,')'])


% Define MissionID to save plots
MISSIONID=char(datetime(datevec(nanmin(fixTable.timestampNum)),'format','yyyyMMdd')); % MC

% Save figure
% print_setup(40,40)
print([path_plots,'Map_CPFtrack_',MISSIONID,'.jpg'],'-djpeg');
print([path_plots,'Map_CPFtrack_',MISSIONID,'.fig']);

% Write KML file for Google Earth
writeKMLTrack(fixTable, sbdFolder);

%%  Printing More Plots, MC
close all

% Define ranges for CanP, CanH and BBVolts cutoff
rangeH=[31.848 39.016];
rangeP=[1.2346    1.3029]*1.0e+03;
battV_threshold=24;

% NOTE: thresholds extracted from the longest (overnight) CPF-MR deployment
% mission='/ParagonTests/MICRORIDER/2021-11-16T19-16-49'; % OVERNIGHT mission, 19 profiles

%% Plot Battery Voltage vs Time

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

hold on
plot([nanmin(fixTable.timestampNum) nanmax(fixTable.timestampNum)], [battV_threshold battV_threshold],'--r');

% Save figure 
print_setup(40,40)
print([path_plots,'BatteryVoltage_CPFtrack_',MISSIONID,'.jpg'],'-djpeg'); % MC

%% Plot Can Pressure vs Time
figure(4)
plot([fixTable.timestampNum], fixTable.canP,'-o');
datetick('x');
ylabel('[mbar]')
title('Can Pressure vs. Time');

hold on
plot([nanmin(fixTable.timestampNum) nanmax(fixTable.timestampNum)], [rangeP(1) rangeP(1)],'--r');
plot([nanmin(fixTable.timestampNum) nanmax(fixTable.timestampNum)], [rangeP(2) rangeP(2)],'--r');

% Save figure
print_setup(40,40)
print([path_plots,'CanPressure_CPFtrack_',MISSIONID,'.jpg'],'-djpeg'); % MC

%% Plot Can Humidity vs Time

figure(5)
plot([fixTable.timestampNum], fixTable.canHum,'-o');
datetick('x');
ylabel('[%]')
title('Can Humidity vs. Time');

hold on
plot([nanmin(fixTable.timestampNum) nanmax(fixTable.timestampNum)], [rangeH(1) rangeH(1)],'--r');
plot([nanmin(fixTable.timestampNum) nanmax(fixTable.timestampNum)], [rangeH(2) rangeH(2)],'--r');

% Save figure
print_setup(40,40)
print([path_plots,'CanHumidity_CPFtrack_',MISSIONID,'.jpg'],'-djpeg'); % MC


