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
% 1) 2022 Mar 31 General clean up for consistency
% 2) add checks for bad messages and bad lat lon
% 3) Starting to add in Josh and Yui stuff

% Specify folder to read SBDS and save kml track and plots
sbdFolder = '\\atlas\projectlibrary\CPF\Operations\2022Apr25\';
plotsFolder=sbdFolder;

sbdFileList = dir([sbdFolder, '*.sbd.*']);

fixNum = 1;
for i=1:size(sbdFileList,1)
    fprintf("Processing SBD number %d: %s\n", i, sbdFileList(i).name)
    
    sbdString = fileread([sbdFileList(i).folder,'/',sbdFileList(i).name]);
    sbdFields = split(sbdString, ',');
    fprintf("Msg: %s\n", sbdString)
    
    if( sbdString(1:4) == "2022" )
        sbdData(fixNum).msgName = sbdFileList(i).name;
        sbdData(fixNum).sbdString = 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);
            sbdData(fixNum).latDD = str2double(latDeg) + (str2double(latMin) / 60.0);
            if(latFields{2}(1) == 'S')
                sbdData(fixNum).latDD = -1.0 * sbdData(fixNum).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}];
            sbdData(fixNum).lonDD = str2double(lonDeg) + (str2double(lonMin) / 60.0);
            if(lonFields{2}(1) == 'W')
                sbdData(fixNum).lonDD = -1.0 * sbdData(fixNum).lonDD;
            end
            
            tempFields = split(sbdFields(27), ' ');
            sbdData(fixNum).profileNum = tempFields(1);
            sbdData(fixNum).timestampString = sbdFields{1};
            sbdData(fixNum).timestampNum = datenum(sbdFields{1},'yyyy-mm-ddTHH:MM:SS');
            sbdData(fixNum).battV = str2double(sbdFields(14));
            
        else
            fullField = sbdFields(4);
            latFields = split(fullField);
            latString = latFields(1,:);
            latDeg = latString{1}(1:2);
            latMin = latString{1}(3:end);
            sbdData(fixNum).latDD = str2double(latDeg) + (str2double(latMin) / 60.0);
            if(latFields{2}(1) == 'S')
                sbdData(fixNum).latDD = -1.0 * sbdData(fixNum).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}];
            sbdData(fixNum).lonDD = str2double(lonDeg) + (str2double(lonMin) / 60.0);
            if(lonFields{2}(1) == 'W')
                sbdData(fixNum).lonDD = -1.0 * sbdData(fixNum).lonDD;
            end
            
            tempFields = split(sbdFields(15), ' ');
            sbdData(fixNum).profileNum = tempFields(1);
            sbdData(fixNum).timestampString = sbdFields{1};
            sbdData(fixNum).timestampNum = datenum(sbdFields{1},'yyyy-mm-ddTHH:MM:SS');
            sbdData(fixNum).dt = datetime(sbdData(fixNum).timestampNum, 'ConvertFrom', 'datenum');
            sbdData(fixNum).battV = str2double(sbdFields(33));
            
            sbdData(fixNum).canP = str2double(sbdFields(35)); % MC
            sbdData(fixNum).canHum = str2double(sbdFields(37)); % MC
        end
        fixNum = fixNum + 1;
    else
        fprintf(1, '***Errror: badly constructed SBD message');
    end
    fprintf(1, '\n');
end

unsortedFixTable = struct2table(sbdData);
index = find(unsortedFixTable.latDD == 0.0);
unsortedFixTable.latDD(index) = NaN;
index = find(unsortedFixTable.lonDD == 0.0);
unsortedFixTable.lonDD(index) = NaN;

fixTable = sortrows(unsortedFixTable, 'timestampNum');
fprintf(1,'\nSBD attachments sorted by time\n')
fprintf(1,'Msg: %s\n', string([fixTable.sbdString]))

temp1 = [fixTable.dt(1:end-1)] - [fixTable.dt(2:end)];
temp1 = [NaN temp1'];
fixTable.profileDuration = temp1';

temp2 = [fixTable.battV(1:end-1)] - [fixTable.battV(2:end)];
temp2 = [NaN temp2'];
fixTable.voltsPerProfile = temp2';

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

figure('Units', 'normalized', 'Position', [0.1 0.1 0.8 0.7]);
chartFig = subplot(3,4,[1 5 9]);
chartFix.Name = 'GPS Fixes (easting, northing)';
ChartFig.NumberTitle = 'off';
chartFig.Units = 'normalized';
chartFig.Position = [0.1 0.1 0.4 0.7];
[mbay, R] = readgeoraster('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(sbdData)
    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')
figTitle = 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(figTitle)
fprintf(1, '%s\n', figTitle);
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([plotsFolder,'Map_CPFtrack_',missionID,'.jpg'],'-djpeg');
%print([plotsFolder,'Map_CPFtrack_',missionID,'.fig']);

% Write KML file for Google Earth
writeKMLTrack(fixTable, sbdFolder, 'KMLTrack.kml');

%%  Printing More Plots, MC
% mission='/ParagonTests/MICRORIDER/2021-11-16T19-16-49'; % OVERNIGHT mission, 19 profiles

%% Plot Battery Voltage vs Time
subplot(3,4,[3 4]);
battV_threshold = 26.5;
numProfilesLeft = (fixTable.battV(end) - battV_threshold) / fixTable.voltsPerProfile(end);
plot(fixTable.dt, fixTable.battV,'-o');
hold on
plot([min(fixTable.dt) max(fixTable.dt)], [battV_threshold battV_threshold],'--r');
ylim([23.5 34]);
ylabel('Hotel Bus Voltage')
title(['Battery Voltage vs. Time   ' 'Number of Profiles to Voltage cutoff = ' num2str(numProfilesLeft)]);
grid on


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

%% Plot Can Pressure vs Time
subplot(3,4, [7 8]);
plot(fixTable.dt, fixTable.canP,'-o');
hold on
rangeP=[1100 1240];
plot([min(fixTable.dt) max(fixTable.dt)], [rangeP(1) rangeP(1)],'--r');
plot([min(fixTable.dt) max(fixTable.dt)], [rangeP(2) rangeP(2)],'--r');
ylabel('Internal Can Pressure [mbar]')
title('Can Pressure vs. Time');
grid on

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

%% Plot Can Humidity vs Time
subplot(3, 4, [11 12]);
plot(fixTable.dt, fixTable.canHum,'-o');
hold on
rangeH = 45;
plot([min(fixTable.dt) max(fixTable.dt)], [rangeH rangeH],'--r');
ylabel('Relative Humidity [%]')
title('Can Humidity vs. Time');
grid on

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


