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
%
% 3/29/2022: Yui Takeshita
% - updated sbdFolder to point to 'Active_Deployment'. Completed
% deployments will need to be renamed into their deployment IDs, and empty
% 'Active_Deployment'. 

% Specify folder to read SBDS and save kml track and plots
% sbdFolder = '\\atlas\ProjectLibrary\CPF\Operations\2021Nov02\';
% sbdFolder = '/Volumes/CPF/Operations/2021Nov02/';
% sbdFolder = '\\atlas\ProjectLibrary\CPF\Operations\2022Feb23\';
sbdFolder = '\\atlas\ProjectLibrary\CPF\Operations\Active_Deployment\'; % YT 3/29/202. 
% sbdFolder = '\\atlas\ProjectLibrary\CPF\Operations\2022Mar29\'; % YT 3/29/202. 
CPF_warning_fn = 'SBD_warning_log.TXT';

webFolder = '\\sirocco\wwwroot\chemsensor\CPF_Current_dep\'; % YT
% webFolder=sbdFolder;
path_plots=sbdFolder;

% fileList = ls([sbdFolder, '*.sbd*']);
fileList    = dir([sbdFolder, '*.sbd*']);
CPF_warning_log = cell(100,3); % sbd file, warning, sbd msg
err_ct      = 0;


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])
    fp  = fullfile(fileList(i).folder, fileList(i).name);
    sbd_str = fileread(fp);
    
    
    % CHECK VALIDITY OF MSG FILE & DATA ( ADD MORE TESTS AS DESIRED)
    tg_str = ~isempty(regexp(sbd_str, 'Lat.+Lon.+BBHealth', 'once')); % bad msg will not have this pattern
    tg_LAT = isempty(regexp(sbd_str,'(?<=Lat,)0000.0','once')); % look for zero lat in str
    
    if ~tg_str
        err_str = sprintf(['WARNING: SBD message is bad or incomplete for %s', ...
            ' and will be excluded from processing'], fileList(i).name);
        err_ct = err_ct + 1;
        CPF_warning_log(err_ct,:) = {fileList(i).name, err_str, sbd_str};
        disp(sbd_str)
        continue
    end
    
    if ~tg_LAT
        err_str = sprintf(['WARNING: SBD message has 000.0 for Lat & Lon', ...
            ' position fixes for %s and will be excluded from plotting'], ...
            fileList(i).name);
        err_ct = err_ct + 1;
        CPF_warning_log(err_ct,:) = {fileList(i).name, err_str, sbd_str};
        disp(sbd_str)
    end
    
    fix(i).sbdString = sbd_str;
    %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
         
%          % if Lat/Lon is 0.00
%          if(~tg_LAT)
%              fix(i).lonDD = NaN;
%              fix(i).latDD = NaN;
%          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

if err_ct > 0 % if errors print warning log to file
    CPF_warning_log = CPF_warning_log(1:err_ct,:);
    D   = CPF_warning_log';
    fid = fopen(fullfile(sbdFolder, CPF_warning_fn),'w');
    fprintf(fid,'%s\t%s\t%s\n',D{:});
    fclose(fid);
end

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

% CHECK FOR BAD (000.000) FIXES IN TABLE  WITH LOGICAl ARRAY
% TEMPOARAY FIX ASSUMING NO REAL ZERO LAT
tgLAT = fixTable.latDD ~= 0 & fixTable.latDD > 30; 



%%
% 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');
X = fixTable.easting(tgLAT); % ONLY GOOD FIXES
Y = fixTable.northing(tgLAT);

plot(X, Y, 'r-+')
plot(X(end), Y(end), 'ro');

xlim(xLimits);
ylim(yLimits);

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
width=40; height=40; ftsize=22;
print_setup(width,height,ftsize)
print([path_plots,'Map_CPFtrack_',MISSIONID,'.jpg'],'-djpeg');
print([path_plots,'Map_CPFtrack_',MISSIONID,'.fig']);
print([webFolder,'Map_CPFtrack.jpg'],'-djpeg'); % YT

% % Write KML file for Google Earth
kmlfile=['CPFTrack_',MISSIONID,'.kml'];
writeKMLTrack(fixTable, sbdFolder, kmlfile);

%%  Printing More Plots
close all

% Define acceptable ranges for CanP, CanH and BBVolts cutoff

battV_threshold=20; % battery threshold cutoff

rangeH(1,:)=[24.742 29.052];
rangeP(1,:)=[1189.38 1856.42]; % 2021-10-19T16-3-9/

rangeH(2,:)=[31.848 39.016];
rangeP(2,:)=[1.2346 1.3029]*1.0e+03; % mission='/ParagonTests/MICRORIDER/2021-11-16T19-16-49'; % OVERNIGHT mission, 19 profiles

rangeH(3,:)=[36.683 40.429];
rangeP(3,:)=[1166.59 1302.12]; % 2022-2-23T16-59-6/

% NOTE: thresholds taken from previous deployment (.eng files)

%% Plot Battery Voltage vs Time

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

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

% Save figure 
width=60; height=30; ftsize=24;
print_setup(width,height,ftsize)
print([path_plots,'BatteryVoltage_CPFtrack_',MISSIONID,'.jpg'],'-djpeg'); % MC
print([webFolder,'BatteryVoltage_CPFtrack.jpg'],'-djpeg'); % YT

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

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

% Save figure
print_setup(width,height,ftsize)
print([path_plots,'CanPressure_CPFtrack_',MISSIONID,'.jpg'],'-djpeg'); % MC
print([webFolder,'CanPressure_CPFtrack.jpg'],'-djpeg'); % YT

%% Plot Can Humidity vs Time

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

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

% Save figure
print_setup(width,height,ftsize)
print([path_plots,'CanHumidity_CPFtrack_',MISSIONID,'.jpg'],'-djpeg'); % MC
print([webFolder,'CanHumidity_CPFtrack.jpg'],'-djpeg'); % YT

