%% Compare model to data - with averaging

% Define high level paths
define_paths;

%% Load TEAMER DATA
D = dir(fullfile(dirSimData, 'Agate_*'));

T_SIM = [];
I_SIM = [];
for i = 1:length(D)
   filename = fullfile(dirSimData,D(i).name);
   % Load time-evolving data
   times = ncread(filename,'Times');
   times = times(1:19,:)'; 
   % convert to datetime
   ts = datetime(times,'InputFormat','uuuu-MM-dd''T''HH:mm:ss','TimeZone','UTC');
   T_SIM = [T_SIM; ts];
   I_SIM = [I_SIM; i*ones(size(ts))];
end   

%% LOAD OBSERVATIONAL DATA
% Load meta data
if ~exist('meta','var')   
    libFolder = fullfile(dirObservationalData, 'lib');
    load(fullfile(libFolder, 'AP2_metadata.mat'));  
    load(fullfile(libFolder, 'AgatePass_SiteBoundary.mat'));
    load(fullfile(libFolder, 'AgatePassBathy.mat'));
    tTests = meta.tTests;
    nTest = length(tTests);
    nLocSK = size(meta.locSK,1);
end

dirSensor = fullfile(dirObservationalData, 'sensors');

% Load station keeping data
if ~exist('SK','var')
    %SK1 = importStationKeepingData('C:\Users\trevo\Dropbox (MREL)\AgatePass\Sounder ADCP\Sounder_ADCP_18Aug2020');
    SK = importStationKeepingData(fullfile(dirSensor,'ADCP_StationKeeping','compiled'), meta);
    SK.D = SK.D + 0.6; % based on previous work, this is the instrument head depth
end

% Load SWIFT data
if ~exist('SWIFT','var')
    SWIFT = importSWIFTdata(fullfile(dirSensor,'ADCP_SWIFT','compiled'), meta);
    SWIFT.TS = repmat(SWIFT.time, 1, size(SWIFT.X,2));
    SWIFT = rmfield(SWIFT, 'time');
    SWIFT.testID = repmat(SWIFT.testID, 1, size(SWIFT.X,2));
    % For better processing, round all swift data to nearest 30 seconds
    tSec = round(seconds(SWIFT.TS-meta.tTests(1)));
    SWIFT.TS = seconds(tSec)+meta.tTests(1);
end

% Load FLOAT SWARM data
if ~exist('FS','var')
    FILES = dir(fullfile(dirSensor,'FloatSwarm','compiled','Float*'));
    for i = 1:length(FILES)
    FS(i) = load(fullfile(FILES(i).folder, FILES(i).name)); 
    end
    clear FILES i;
    
    % Rename variables in FS to match SWIFT and SK name convention.
    for i = 1:length(FS)
        FS(i).X = FS(i).xSS;
        FS(i).Y = FS(i).ySS;
        FS(i).Z = FS(i).zSS;
        FS(i).UX = FS(i).UXS;
        FS(i).UY = FS(i).UYS;
        FS(i).TS = repmat((FS(i).tRef+seconds(FS(i).tTestSet))',1,size(FS(i).X,2));
    end
    FS = rmfield(FS,{'UXS','UYS','xSS','ySS','zSS'});
    
end

% Load CTD data
if ~exist('CTD','var')
    FILES = dir(fullfile(dirSensor,'CTD','compiled','CC*'));
    for i = 1:length(FILES)
        CTD(i) = load(fullfile(FILES(i).folder, FILES(i).name));
    end
    CTD = CTD(~[CTD.IsInvalid]);
    CTD = CTD([3:7,9:12]); % Grab relevant profiles (skips doubles, and 1st)   
    castTimes = datetime([CTD.CastTimeUtc]', 'TimeZone','UTC');
    for i = 1:length(CTD)
        meanSOS(i) = mean(CTD(i).Sound_velocity);
    end
    %sos =  interp1(castTimes, meanSOS, meta.tTests(testID)+minutes(10));
end 

%% DEFINE THE STATION-KEEPING LOCATIONS IN LOCAL COORDINATES
latRef = meta.locSK(3,1);
lonRef = meta.locSK(3,2);

% Define station keeping locations (local coordinates)
[locSK(:,2), locSK(:,1)] = geodetic2ned(meta.locSK(:,1),meta.locSK(:,2),0,latRef,lonRef,0,referenceEllipsoid('wgs84'));

%% TIME AVERAGE OBSERVATIONAL DATA - FLOATS

% Storage for averaged data. 
FS_30 = rmfield(FS, {'TEMP', 'addrAllDev','smoothSpan','tRef','tTestSet'});
FS_60 = FS_30;
FS_2m = FS_30;

for i = 1:nTest
   fs = FS(i);
   
   % 30 second averaging
   [bin30] = discretize(fs.tTestSet, fs.tTestSet(1):30:fs.tTestSet(end));
   binIDs = unique(bin30); 
   nSeg = numel(unique(bin30));
   x = nan(nSeg,size(fs.UX,2));
   y = x; z = x; ux= x; uy = x;
   t = NaT(nSeg,size(fs.UX,2),'TimeZone','America/Los_Angeles');
   for j = 1:nSeg
       bSub = bin30 == binIDs(j);
       x(j,:) = nanmean(fs.X(bSub,:),1);
       y(j,:) = nanmean(fs.Y(bSub,:),1);
       z(j,:) = nanmean(fs.Z(bSub,:),1);
       ux(j,:) = nanmean(fs.UX(bSub,:),1);
       uy(j,:) = nanmean(fs.UY(bSub,:),1);
       t(j,:) =  min(fs.TS(bSub,:));
   end

   FS_30(i).X = x;
   FS_30(i).Y = y;
   FS_30(i).Z = z;
   FS_30(i).UX = ux;
   FS_30(i).UY = uy;
   FS_30(i).TS = t + seconds(15);
   
   % 1 minute averaging
   [bin60] = discretize(fs.tTestSet, fs.tTestSet(1):60:fs.tTestSet(end));
   binIDs = unique(bin60);
   nSeg = numel(unique(bin60));
   x = nan(nSeg,size(fs.UX,2));
   y = x; z = x; ux = x; uy = x;
   t = NaT(nSeg,size(fs.UX,2),'TimeZone','America/Los_Angeles');
   for j = 1:nSeg
       bSub = bin60 == binIDs(j);
       x(j,:) = nanmean(fs.X(bSub,:),1);
       y(j,:) = nanmean(fs.Y(bSub,:),1);
       z(j,:) = nanmean(fs.Z(bSub,:),1);
       ux(j,:) = nanmean(fs.UX(bSub,:),1);
       uy(j,:) = nanmean(fs.UY(bSub,:),1);
       t(j,:) =  min(fs.TS(bSub,:));
   end

   FS_60(i).X = x;
   FS_60(i).Y = y;
   FS_60(i).Z = z;
   FS_60(i).UX = ux;
   FS_60(i).UY = uy;
   FS_60(i).TS = t + seconds(30);
   
   % 2 minute averaging
   [bin2m] = discretize(fs.tTestSet, fs.tTestSet(1):120:fs.tTestSet(end));
   binIDs = unique(bin2m);
   nSeg = numel(unique(bin2m));
   x = nan(nSeg,size(fs.UX,2));
   y = x; z = x; ux = x; uy = x;
   t = NaT(nSeg,size(fs.UX,2),'TimeZone','America/Los_Angeles');
   for j = 1:nSeg
       bSub = bin2m == binIDs(j);
       x(j,:) = nanmean(fs.X(bSub,:),1);
       y(j,:) = nanmean(fs.Y(bSub,:),1);
       z(j,:) = nanmean(fs.Z(bSub,:),1);
       ux(j,:) = nanmean(fs.UX(bSub,:),1);
       uy(j,:) = nanmean(fs.UY(bSub,:),1);
       t(j,:) =  min(fs.TS(bSub,:));
   end

   FS_2m(i).X = x;
   FS_2m(i).Y = y;
   FS_2m(i).Z = z;
   FS_2m(i).UX = ux;
   FS_2m(i).UY = uy;
   FS_2m(i).TS = t + minutes(1);
end

% Check if valid samples
for i = 1:9
    FS(i).nValid = sum(~isnan(FS(i).UX(:)));
    FS(i).nSample = numel(FS(i).UX(:));
    
    FS_30(i).nValid = sum(~isnan(FS_30(i).UX(:)));
    FS_30(i).nSample = numel(FS_30(i).UX(:));
    
    FS_60(i).nValid = sum(~isnan(FS_60(i).UX(:)));
    FS_60(i).nSample = numel(FS_60(i).UX(:));
    
    FS_2m(i).nValid = sum(~isnan(FS_2m(i).UX(:)));
    FS_2m(i).nSample = numel(FS_2m(i).UX(:));
end

fs_Samp = cat(1, FS.nSample);
fs_30_Samp = cat(1, FS_30.nSample);
fs_60_Samp = cat(1, FS_60.nSample);
fs_2m_Samp = cat(1, FS_2m.nSample);

%% DOUBLE-CHECK AVERAGING - Processing QC
if 1 == 0
    f1 = figure(1); clf(f1);
    ax = buildAxes(f1,4,1); set(ax, 'NextPlot','add');
    cLines = lines(24);
    for i = 1:length(FS)
        % Get high-res (30 s) samples from SWIFT
        for j = 1:size(FS(i).X,2)
            for k = 1:length(ax)
                cla(ax(k));
            end
        tSub = FS(i).TS(:,j); % time in seconds relative to beginning of test day
        xSub = FS(i).X(:,j);
        ySub = FS(i).Y(:,j);
        uxSub = FS(i).UX(:,j);
        uySub = FS(i).UY(:,j);

        plot(ax(1), tSub, xSub, '.', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, '.', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, '.', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, '.', 'Color', cLines(i,:)); 

        tSub = FS_60(i).TS(:,j); % time in seconds relative to beginning of test day
        xSub = FS_60(i).X(:,j);
        ySub = FS_60(i).Y(:,j);
        uxSub = FS_60(i).UX(:,j);
        uySub = FS_60(i).UY(:,j);

        plot(ax(1), tSub, xSub, '+', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, '+', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, '+', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, '+', 'Color', cLines(i,:)); 

        tSub = FS_2m(i).TS(:,j); % time in seconds relative to beginning of test day
        xSub = FS_2m(i).X(:,j);
        ySub = FS_2m(i).Y(:,j);
        uxSub = FS_2m(i).UX(:,j);
        uySub = FS_2m(i).UY(:,j);

        plot(ax(1), tSub, xSub, 'x', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, 'x', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, 'x', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, 'x', 'Color', cLines(i,:));
        output = input('Enter to continue. Any other key to stop.','s');
        if ~isempty(output)
            break;
        end
    end
    end
end

% Check times
if 1 == 0
    i = 1;
    f1 = figure(1); clf(f1); ax = gca;
    plot(ax, FS(i).TS(:,1), zeros(size(FS(i).TS(:,1))), '.');
    hold on; 
    plot(ax, FS_30(i).TS(:,1), 1+zeros(size(FS_30(i).TS(:,1))), '.');
    plot(ax, FS_60(i).TS(:,1), 2+zeros(size(FS_60(i).TS(:,1))), '.');
    plot(ax, FS_2m(i).TS(:,1), 3+zeros(size(FS_2m(i).TS(:,1))), '.');
    ylim([-5, 10]);
end

%% SWIFTS
SWIFT_ids = unique(SWIFT.devID);
tAll = seconds(SWIFT.TS(:,1)-meta.tTests(1));

% Build data for storing
X60 = []; Y60 = []; Z60 = []; UX60 = []; UY60 = []; UZ60 = []; T60 = []; dev60 = [];
X2m = []; Y2m = []; Z2m = []; UX2m = []; UY2m = []; UZ2m = []; T2m = []; dev2m = [];

for i = 1:length(SWIFT_ids)
    % Get samples from one SWIFT
    bDev = SWIFT.devID == SWIFT_ids(i);
    tSub = tAll(bDev); % time in seconds relative to beginning of test day
    xSub = SWIFT.X(bDev,:);
    ySub = SWIFT.Y(bDev,:);
    zSub = SWIFT.Z(bDev,:);
    uxSub = SWIFT.UX(bDev,:);
    uySub = SWIFT.UY(bDev,:);
    
    % Binning 
    tNom = 0:60:tSub(end)-mod(tSub(end),60)+60;
    bin60 = discretize(tSub, tNom);
    binIDs = unique(bin60);
    nSeg60 =  numel(unique(bin60));
    x = nan(nSeg60, size(SWIFT.UX, 2));
    y = x; z = x; ux= x; uy = x; 
    t = nan(nSeg60, 1);
    
    % perform averaging
    for j = 1:nSeg60
        if sum(bin60==binIDs(j))>1
           % t2m(j) = tNom(binIDs(j))+30; % using nominal times
           t(j) = nanmean(tSub(bin60==binIDs(j)));
           x(j,:) = nanmean(xSub(bin60==binIDs(j),:),1);
           y(j,:) = nanmean(ySub(bin60==binIDs(j),:),1);
           z(j,:) = nanmean(zSub(bin60==binIDs(j),:),1);
           ux(j,:) = nanmean(uxSub(bin60==binIDs(j),:),1);
           uy(j,:) = nanmean(uySub(bin60==binIDs(j),:),1);
        end
    end
    
    X60 = cat(1, X60, x);
    Y60 = cat(1, Y60, y);
    Z60 = cat(1, Z60, z);
    UX60 = cat(1, UX60, ux);
    UY60 = cat(1, UY60, uy);
    T60 = cat(1, T60, t);
    dev60 = cat(1, dev60, SWIFT_ids(i).*ones(size(t)));

    % Get sub-indeces for 2 minute averaging
    tNom = 0:120:tSub(end)-mod(tSub(end),120)+120;
    bin2m = discretize(tSub, tNom);
    binIDs = unique(bin2m);
    nSeg2m =  numel(unique(bin2m));
    x = nan(nSeg2m, size(SWIFT.UX, 2));
    y = x; z = x; ux= x; uy = x; 
    t = nan(nSeg2m, 1);
    
    % perform averaging
    for j = 1:nSeg2m
        if sum(bin2m==binIDs(j))>1
           % t2m(j) = tNom(binIDs(j))+60; % using nominal times
           t(j) = nanmean(tSub(bin2m==binIDs(j)));
           x(j,:) = nanmean(xSub(bin2m==binIDs(j),:),1);
           y(j,:) = nanmean(ySub(bin2m==binIDs(j),:),1);
           z(j,:) = nanmean(zSub(bin2m==binIDs(j),:),1);
           ux(j,:) = nanmean(uxSub(bin2m==binIDs(j),:),1);
           uy(j,:) = nanmean(uySub(bin2m==binIDs(j),:),1);
        end
    end
    
    X2m = cat(1, X2m, x);
    Y2m = cat(1, Y2m, y);
    Z2m = cat(1, Z2m, z);
    UX2m = cat(1, UX2m, ux);
    UY2m = cat(1, UY2m, uy);
    T2m = cat(1, T2m, t);
    dev2m = cat(1, dev2m, SWIFT_ids(i).*ones(size(t)));
end    

% Sort the outputs by time and assemble in SWIFT structure
[~, IA] = sort(T60);
SWIFT_60.X = X60(IA,:);
SWIFT_60.Y = Y60(IA,:);
SWIFT_60.Z = Z60(IA,:);
SWIFT_60.UX = UX60(IA,:);
SWIFT_60.UY = UY60(IA,:);
SWIFT_60.TS = repmat(seconds(T60(IA)-mod(T60(IA),30)) + meta.tTests(1),1, size(SWIFT_60.X,2));
SWIFT_60.devID = dev60(IA);
SWIFT_60.testID = 0*dev60(IA); % for later, but constructed here to keep things together in the strcutre

[~, IA] = sort(T2m);
SWIFT_2m.X = X2m(IA,:);
SWIFT_2m.Y = Y2m(IA,:);
SWIFT_2m.Z = Z2m(IA,:);
SWIFT_2m.UX = UX2m(IA,:);
SWIFT_2m.UY = UY2m(IA,:);
SWIFT_2m.TS = repmat(seconds(T2m(IA)-mod(T2m(IA),30)) + meta.tTests(1), 1, size(SWIFT_2m.X,2));
SWIFT_2m.devID = dev2m(IA);
SWIFT_2m.testID = 0*dev2m(IA); % for later

% For convenience, get test IDs for the averaged data too
for i = 1:length(meta.tTests)
    SWIFT_60.testID(SWIFT_60.TS(:,1) > meta.tTests(i)-minutes(5) &...
        SWIFT_60.TS(:,1) < meta.tTests(i)+minutes(25)) = i;
    SWIFT_2m.testID(SWIFT_2m.TS(:,1) > meta.tTests(i)-minutes(5) &...
        SWIFT_2m.TS(:,1) < meta.tTests(i)+minutes(25)) = i;
end

%% DOUBLE-CHECK AVERAGING - Processing QC
if 1 == 0
    f1 = figure(1); clf(f1);
    ax = buildAxes(f1,4,1); set(ax, 'NextPlot','add');
    zLayer = 1;
    cLines = lines(7);
    for i = 1:length(SWIFT_ids)
        % Get high-res (30 s) samples from SWIFT
        bDev = SWIFT.devID == SWIFT_ids(i);
        tSub = SWIFT.TS(bDev,zLayer); % time in seconds relative to beginning of test day
        xSub = SWIFT.X(bDev,zLayer);
        ySub = SWIFT.Y(bDev,zLayer);
        uxSub = SWIFT.UX(bDev,zLayer);
        uySub = SWIFT.UY(bDev,zLayer);

        plot(ax(1), tSub, xSub, '.', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, '.', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, '.', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, '.', 'Color', cLines(i,:)); 

        bDev = SWIFT_60.devID == SWIFT_ids(i);
        tSub = SWIFT_60.TS(bDev,zLayer); % time in seconds relative to beginning of test day
        xSub = SWIFT_60.X(bDev,zLayer);
        ySub = SWIFT_60.Y(bDev,zLayer);
        uxSub = SWIFT_60.UX(bDev,zLayer);
        uySub = SWIFT_60.UY(bDev,zLayer);

        plot(ax(1), tSub, xSub, '+', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, '+', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, '+', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, '+', 'Color', cLines(i,:)); 

        bDev = SWIFT_2m.devID == SWIFT_ids(i);
        tSub = SWIFT_2m.TS(bDev,zLayer); % time in seconds relative to beginning of test day
        xSub = SWIFT_2m.X(bDev,zLayer);
        ySub = SWIFT_2m.Y(bDev,zLayer);
        uxSub = SWIFT_2m.UX(bDev,zLayer);
        uySub = SWIFT_2m.UY(bDev,zLayer);

        plot(ax(1), tSub, xSub, 'x', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, 'x', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, 'x', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, 'x', 'Color', cLines(i,:)); 
        output = input('Enter to continue. Any other key to stop.','s');
        if ~isempty(output)
            break;
        end
    end
end 

% Check times
if 1 == 0
    f1 = figure(1); clf(f1); ax = gca;
    plot(ax, SWIFT.TS(:,1), zeros(size(SWIFT.TS(:,1))), '.');
    hold on; 
    plot(ax, SWIFT_60.TS(:,1), 1+zeros(size(SWIFT_60.TS(:,1))), '.');
    plot(ax, SWIFT_2m.TS(:,1), 2+zeros(size(SWIFT_2m.TS(:,1))), '.');
    ylim([-5, 10]);
end

%% STATION KEEPING
% Define station-keeping location
SK.locID = nan(size(SK.TS));
for i = 1:length(meta.locSK)
    sepDist = sqrt((SK.X(:) - locSK(i,1)).^2 + (SK.Y(:) - locSK(i,2)).^2);
    SK.locID(sepDist < 40) = i;
end
tAll = seconds(SK.TS(:,1) - meta.tTests(1));

% Build data for storing
X30 = []; Y30 = []; Z30 = []; UX30 = []; UY30 = []; UZ30 = []; T30 = []; loc30 = [];
X60 = []; Y60 = []; Z60 = []; UX60 = []; UY60 = []; UZ60 = []; T60 = []; loc60 = [];
X2m = []; Y2m = []; Z2m = []; UX2m = []; UY2m = []; UZ2m = []; T2m = []; loc2m = [];
X5m = []; Y5m = []; Z5m = []; UX5m = []; UY5m = []; UZ5m = []; T5m = []; loc5m = [];

for i = 1:length(meta.locSK)
    % Get samples from location
    bLoc = SK.locID(:,1) == i; 
    tSub = tAll(bLoc); % time in seconds relative to beginning of test day
    xSub = SK.X(bLoc,:);
    ySub = SK.Y(bLoc,:);
    zSub = SK.Z(bLoc,:);
    uxSub = SK.UX(bLoc,:);
    uySub = SK.UY(bLoc,:);
    
    % Binning - 30 seconds averaging
    tNom = 0:30:nanmax(tSub)-mod(nanmax(tSub),30)+30;
    bin30 = discretize(tSub, tNom);
    binIDs = unique(bin30);
    nSeg30 =  numel(unique(bin30));
    x = nan(nSeg30, size(SK.UX, 2));
    y = x; z = x; ux= x; uy = x; 
    t = nan(nSeg30, 1);
    
    % perform averaging
    for j = 1:nSeg30
        if sum(bin30==binIDs(j))>1
           % t2m(j) = tNom(binIDs(j))+30; % using nominal times
           t(j) = nanmean(tSub(bin30==binIDs(j)));
           x(j,:) = nanmean(xSub(bin30==binIDs(j),:),1);
           y(j,:) = nanmean(ySub(bin30==binIDs(j),:),1);
           z(j,:) = nanmean(zSub(bin30==binIDs(j),:),1);
           ux(j,:) = nanmean(uxSub(bin30==binIDs(j),:),1);
           uy(j,:) = nanmean(uySub(bin30==binIDs(j),:),1);
        end
    end   
    X30 = cat(1, X30, x);
    Y30 = cat(1, Y30, y);
    Z30 = cat(1, Z30, z);
    UX30 = cat(1, UX30, ux);
    UY30 = cat(1, UY30, uy);
    T30 = cat(1, T30, t);
    loc30 = cat(1, loc30, i.*ones(size(t)));
    
    % Binning - 60 second averaging
    tNom = 0:60:nanmax(tSub)-mod(nanmax(tSub),60)+60;
    bin60 = discretize(tSub, tNom);
    binIDs = unique(bin60);
    nSeg60 =  numel(unique(bin60));
    x = nan(nSeg60, size(SK.UX, 2));
    y = x; z = x; ux= x; uy = x; 
    t = nan(nSeg60, 1);
    
    % perform averaging
    for j = 1:nSeg60
        if sum(bin60==binIDs(j))>1
           % t2m(j) = tNom(binIDs(j))+30; % using nominal times
           t(j) = nanmean(tSub(bin60==binIDs(j)));
           x(j,:) = nanmean(xSub(bin60==binIDs(j),:),1);
           y(j,:) = nanmean(ySub(bin60==binIDs(j),:),1);
           z(j,:) = nanmean(zSub(bin60==binIDs(j),:),1);
           ux(j,:) = nanmean(uxSub(bin60==binIDs(j),:),1);
           uy(j,:) = nanmean(uySub(bin60==binIDs(j),:),1);
        end
    end   
    X60 = cat(1, X60, x);
    Y60 = cat(1, Y60, y);
    Z60 = cat(1, Z60, z);
    UX60 = cat(1, UX60, ux);
    UY60 = cat(1, UY60, uy);
    T60 = cat(1, T60, t);
    loc60 = cat(1, loc60, i.*ones(size(t)));

    % Binning - 2-minute averaging
    tNom = 0:120:nanmax(tSub)-mod(nanmax(tSub),120)+120;
    bin2m = discretize(tSub, tNom);
    binIDs = unique(bin2m);
    nSeg2m =  numel(unique(bin2m));
    x = nan(nSeg2m, size(SK.UX, 2));
    y = x; z = x; ux= x; uy = x; 
    t = nan(nSeg2m, 1);
    
    % perform averaging
    for j = 1:nSeg2m
        if sum(bin2m==binIDs(j))>1
           % t2m(j) = tNom(binIDs(j))+30; % using nominal times
           t(j) = nanmean(tSub(bin2m==binIDs(j)));
           x(j,:) = nanmean(xSub(bin2m==binIDs(j),:),1);
           y(j,:) = nanmean(ySub(bin2m==binIDs(j),:),1);
           z(j,:) = nanmean(zSub(bin2m==binIDs(j),:),1);
           ux(j,:) = nanmean(uxSub(bin2m==binIDs(j),:),1);
           uy(j,:) = nanmean(uySub(bin2m==binIDs(j),:),1);
        end
    end   
    X2m = cat(1, X2m, x);
    Y2m = cat(1, Y2m, y);
    Z2m = cat(1, Z2m, z);
    UX2m = cat(1, UX2m, ux);
    UY2m = cat(1, UY2m, uy);
    T2m = cat(1, T2m, t);
    loc2m = cat(1, loc2m, i.*ones(size(t)));
    
    % Binning - full average at station
    x5m = nan(nTest, size(SK.UX, 2));
    y5m = x5m; z5m = x5m; ux5m= x5m; uy5m = x5m; 
    t5m = nan(nTest, 1);
    
    % perform averaging
    for j = 1:nTest       
       % t5m(j) = tNom(binIDs(j))+30; % using nominal times
       bSub = SK.testID(:,1) == j; bSub = bSub(bLoc);
       t5m(j) = nanmean(tSub(bSub));
       x5m(j,:) = nanmean(xSub(bSub,:),1);
       y5m(j,:) = nanmean(ySub(bSub,:),1);
       z5m(j,:) = nanmean(zSub(bSub,:),1);
       ux5m(j,:) = nanmean(uxSub(bSub,:),1);
       uy5m(j,:) = nanmean(uySub(bSub,:),1);
    end   
    X5m = cat(1, X5m, x5m);
    Y5m = cat(1, Y5m, y5m);
    Z5m = cat(1, Z5m, z5m);
    UX5m = cat(1, UX5m, ux5m);
    UY5m = cat(1, UY5m, uy5m);
    T5m = cat(1, T5m, t5m);
    loc5m = cat(1, loc5m, i.*ones(size(t5m)));   
end    

% Sort the outputs by time and append to SK structure
[~, IA] = sort(T30);
SK_30.X = X30(IA,:);
SK_30.Y = Y30(IA,:);
SK_30.Z = Z30(IA,:);
SK_30.UX = UX30(IA,:);
SK_30.UY = UY30(IA,:);
SK_30.TS = repmat(seconds(T30(IA)-mod(T30(IA),30)) + meta.tTests(1), 1, size(SK_30.X,2));
SK_30.locID = loc30(IA);
SK_30.testID = 0*loc30(IA); % will be corrected later

[~, IA] = sort(T60);
SK_60.X = X60(IA,:);
SK_60.Y = Y60(IA,:);
SK_60.Z = Z60(IA,:);
SK_60.UX = UX60(IA,:);
SK_60.UY = UY60(IA,:);
SK_60.TS = repmat(seconds(T60(IA)-mod(T60(IA),30)) + meta.tTests(1), 1, size(SK_60.X,2));
SK_60.locID = loc60(IA);
SK_60.testID = 0*loc60(IA); % will be corrected later

[~, IA] = sort(T2m);
SK_2m.X = X2m(IA,:);
SK_2m.Y = Y2m(IA,:);
SK_2m.Z = Z2m(IA,:);
SK_2m.UX = UX2m(IA,:);
SK_2m.UY = UY2m(IA,:);
SK_2m.TS = repmat(seconds(T2m(IA)-mod(T2m(IA),30)) + meta.tTests(1), 1, size(SK_2m.X,2));
SK_2m.locID = loc2m(IA);
SK_2m.testID = 0*loc2m(IA); % will be corrected later

[~, IA] = sort(T5m);
SK_5m.X = X5m(IA,:);
SK_5m.Y = Y5m(IA,:);
SK_5m.Z = Z5m(IA,:);
SK_5m.UX = UX5m(IA,:);
SK_5m.UY = UY5m(IA,:);
SK_5m.TS = repmat(seconds(T5m(IA)-mod(T5m(IA),30)) + meta.tTests(1), 1, size(SK_5m.X,2));
SK_5m.locID = loc5m(IA);
SK_5m.testID = 0*loc5m(IA); % will be corrected later

% For convenience, get test IDs for the averaged data too
for i = 1:length(meta.tTests)
    SK_30.testID(SK_30.TS(:,1) > meta.tTests(i)-minutes(5) &...
        SK_30.TS(:,1) < meta.tTests(i)+minutes(25)) = i;   
    SK_60.testID(SK_60.TS(:,1) > meta.tTests(i)-minutes(5) &...
        SK_60.TS(:,1) < meta.tTests(i)+minutes(25)) = i;
    SK_2m.testID(SK_2m.TS(:,1) > meta.tTests(i)-minutes(5) &...
        SK_2m.TS(:,1) < meta.tTests(i)+minutes(25)) = i;
    SK_5m.testID(SK_5m.TS(:,1) > meta.tTests(i)-minutes(5) &...
        SK_5m.TS(:,1) < meta.tTests(i)+minutes(25)) = i;
end

%% DOUBLE-CHECK AVERAGING - Processing QC

if 1 == 0
    f1 = figure(1); clf(f1);
    ax = buildAxes(f1,4,1); set(ax, 'NextPlot','add');
    zLayer = 1;
    cLines = lines(7);
    for i = 1:length(meta.locSK)
        % Get high-res (30 s) samples from SWIFT
        bLoc = SK.locID(:,1) == i;
        tSub = SK.TS(bLoc,zLayer); % time in seconds relative to beginning of test day
        xSub = SK.X(bLoc,zLayer);
        ySub = SK.Y(bLoc,zLayer);
        uxSub = SK.UX(bLoc,zLayer);
        uySub = SK.UY(bLoc,zLayer);

        plot(ax(1), tSub, xSub, '.', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, '.', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, '.', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, '.', 'Color', cLines(i,:)); 

        bLoc = SK_60.locID == i;
        tSub = SK_60.TS(bLoc,zLayer); % time in seconds relative to beginning of test day
        xSub = SK_60.X(bLoc,zLayer);
        ySub = SK_60.Y(bLoc,zLayer);
        uxSub = SK_60.UX(bLoc,zLayer);
        uySub = SK_60.UY(bLoc,zLayer);

        plot(ax(1), tSub, xSub, '+', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, '+', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, '+', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, '+', 'Color', cLines(i,:)); 
        
        bLoc = SK_5m.locID == i;
        tSub = SK_5m.TS(bLoc,zLayer); % time in seconds relative to beginning of test day
        xSub = SK_5m.X(bLoc,zLayer);
        ySub = SK_5m.Y(bLoc,zLayer);
        uxSub = SK_5m.UX(bLoc,zLayer);
        uySub = SK_5m.UY(bLoc,zLayer);

        plot(ax(1), tSub, xSub, 's', 'Color', cLines(i,:)); 
        plot(ax(2), tSub, ySub, 's', 'Color', cLines(i,:)); 
        plot(ax(3), tSub, uxSub, 's', 'Color', cLines(i,:)); 
        plot(ax(4), tSub, uySub, 's', 'Color', cLines(i,:)); 
        linkaxes(ax,'x');
        output = input('Enter to continue. Any other key to stop.','s');
        if ~isempty(output)
            break;
        end
    end
end

% Check times
if 1 == 0
    f1 = figure(1); clf(f1); ax = gca;
    plot(ax, SK.TS(:,1), zeros(size(SK.TS(:,1))), '.');
    hold on; 
    plot(ax, SK_30.TS(:,1), 1+zeros(size(SK_30.TS(:,1))), '.');
    plot(ax, SK_60.TS(:,1), 2+zeros(size(SK_60.TS(:,1))), '.');
    plot(ax, SK_2m.TS(:,1), 3+zeros(size(SK_2m.TS(:,1))), '.');
    plot(ax, SK_5m.TS(:,1), 4+zeros(size(SK_5m.TS(:,1))), '.');
    
end

%% Get unique times of all data points
% Append all field data times stamps
FS_TS = cat(2,FS.TS);
TQ = [SWIFT.TS(:); SK.TS(:); FS_TS(:)];
% Round to 30 secinds to align with model time steps.
secOff = mod(TQ.Second,30);
TQ.Second = TQ.Second-secOff;
% Identify unique time stamps
TQ = unique(TQ);
% Locate the intersection of field data and model time-steps
[~,IA,IB] = intersect(T_SIM, TQ);
% Fill in short gaps and get times on front and back of surveys. 
bAll = false(size(T_SIM));
bAll([IA; IA-1; IA-2; IA+1; IA+2]) = 1;
% But don't add time on front end (grabs model data from wrong day)
bAll([IA(1)+[-1, -2], IA(end)+[1,2]]) = 0;
dbAll = [0; diff(bAll)];
% Identify start and end time steps
tStart = T_SIM(dbAll==1);
tEnd = T_SIM(dbAll==-1);
% Assign survey number [for each timestep T_SIM]
survey_num = cumsum([0; diff(bAll)]>0);
survey_num(~bAll) = 0;
% Index of time steps within each hour (model data)
iRel =  repmat(1:length(ts),1,length(D))';
% For SWIFT and SK, assign survey number
SWIFT.survey_num = repmat(SWIFT.testID, 1, size(SWIFT.X,2)); 
SK.survey_num = SK.testID;

%% Convert FIELD DATA to local coordinate frame
% Define reference location. 
latRef = meta.locSK(3,1);
lonRef = meta.locSK(3,2);

% Define station keeping locations (local coordinates)
[locSK(:,2), locSK(:,1)] = geodetic2ned(meta.locSK(:,1),meta.locSK(:,2),0,latRef,lonRef,0,referenceEllipsoid('wgs84'));

% Need coastlines for plotting
[yCLwest, xCLwest] =  geodetic2ned(CLwest(:,1), CLwest(:,2), 0,...
    latRef, lonRef, 0, referenceEllipsoid('wgs84'));
[yCLeast, xCLeast] =  geodetic2ned(CLeast(:,1), CLeast(:,2), 0,...
    latRef, lonRef, 0, referenceEllipsoid('wgs84'));

% Bathymetry (natively in UTM, convert to lat lon, then to local 
xB = bathyData.xB;
yB = bathyData.yB;
zB = -bathyData.zB; % Our bathymetry 
[latBs, lonBs] = utm2ll(xB, yB, 10);
[yBs, xBs] = geodetic2ned(latBs, lonBs, 0, latRef, lonRef, 0, referenceEllipsoid('wgs84'));
% Crop section we care about
xCrop = [-1000, 1500];
yCrop = [-1000, 2500];
bInside = xBs>xCrop(1) & xBs<xCrop(2) & yBs>yCrop(1) & yBs<yCrop(2);
xBs = xBs(bInside);
yBs = yBs(bInside);
zB = zB(bInside);
G_B = scatteredInterpolant(xBs, yBs, zB,'linear','none');
kBounds = boundary(xBs, yBs,1);

% Compiled Observational data is all in [m] reference to SK3. Do not
% require any adjustment here. 

%% INITIALIZE SIMULATION PROPERTIES - TIME-INVARIENT VARIABLES
filename = fullfile(dirSimData,D(1).name);

% Load constant grid parameters
x_n = ncread(filename,'x');
y_n = ncread(filename,'y');
%lat_n = ncread(filename,'lat');
%lon_n = ncread(filename,'lon');
h_n = ncread(filename,'h');
siglay_n = ncread(filename,'siglay'); % centers [1x20]
siglev_n = ncread(filename,'siglev'); % edges   [1x21]

x_c = ncread(filename,'xc');
y_c = ncread(filename,'yc');
%lat_c = ncread(filename,'latc');
%lon_c = ncread(filename,'lonc');
h_c = ncread(filename,'h_center');
siglay_c = ncread(filename,'siglay_center'); % [-1, 0]
siglev_c = ncread(filename,'siglev_center');  
nLayer = size(siglay_c,2);

% [x, y] = geodetic2ned(lat_n,lon_n,0,latRef,lonRef,0,referenceEllipsoid('wgs84'));
% Convert reference location to zonal offset. 
[xRef, yRef] = ll2utm(latRef, lonRef);

% Sim node coordinates: local
xn_l = x_n-xRef;
yn_l = y_n-yRef;
% Sim center coordinates: local
xc_l = x_c-xRef;
yc_l = y_c-yRef;

%% FOR EACH SURVEY, GET SIM DATA AND INTERPOLATE

% ASSEMBLED INDECES - SUMMARY
%      All with dimensions [1320 1]
% T_SIM - start time-step of all time-steps simulated (across all files)
% I_SIM - index of SIMULATION FILE (by hour)
% I_REL - index (out of n=120 timesteps) within the simulation hour
% survey_num - survey number (0 if timestep not used in any survey) 

% FOR EACH SURVEY, ASSEMBLE TIME-VARYING DATA AND INTERPOLATE

% NOTE: the measured variables remain identical. Only the query samples
% need to be added.
SWIFT.UXQ = nan*(SWIFT.UX); SWIFT.UYQ = nan*SWIFT.UY;  
SWIFT_60.UXQ = nan*(SWIFT_60.UX); SWIFT_60.UYQ = nan*SWIFT_60.UY;
SWIFT_2m.UXQ = nan*(SWIFT_2m.UX); SWIFT_2m.UYQ = nan*SWIFT_2m.UY; 

SK.UXQ = nan*(SK.UX); SK.UYQ = nan*SK.UY; 
SK_30.UXQ = nan*(SK_30.UX); SK_30.UYQ = nan*SK_30.UY; 
SK_60.UXQ = nan*(SK_60.UX); SK_60.UYQ = nan*SK_60.UY;
SK_2m.UXQ = nan*(SK_2m.UX); SK_2m.UYQ = nan*SK_2m.UY; 
SK_5m.UXQ = nan*(SK_5m.UX); SK_5m.UYQ = nan*SK_5m.UY; 
 
for i = 1:9
    FS(i).UXQ = nan*FS(i).UX; FS(i).UYQ = nan*FS(i).UY;
    FS_30(i).UXQ = nan*FS_30(i).UX; FS_30(i).UYQ = nan*FS_30(i).UY;
    FS_60(i).UXQ = nan*FS_60(i).UX; FS_60(i).UYQ = nan*FS_60(i).UY;
    FS_2m(i).UXQ = nan*FS_2m(i).UX; FS_2m(i).UYQ = nan*FS_2m(i).UY;
end

% For comparing depth data, interpolated here. 
SK.DQ = nan*SK.D;
SWIFT.DQ = nan*SWIFT.D;

for iSurvey = 1:9
    fprintf('>>>    Processing Survey %i   <<<\n ', iSurvey);
    cSurvey = tic;
    % Initialize the data structure
    bSurvey = survey_num == iSurvey;
    ts = T_SIM(bSurvey); % full-res data set
    TS = repmat(permute(ts,[3,2,1]),size(xc_l,1),nLayer,1);
    XS = repmat(xc_l,1,nLayer,length(ts));
    YS = repmat(yc_l,1,nLayer,length(ts));
    ZS = 0*YS; % initialize
    DS = 0*YS; % ensure no interpolation underground.
    UXS = 0*YS; % initialize
    UYS = 0*YS; % initialize
    
    % Load time-varying data
    fprintf(' >> Loading simulation data ...');
    tstart = tic;
    sim = I_SIM(bSurvey);
    iSim = unique(sim); % For stitching surveys over multiple sim_hour
    
    for i = 1:length(iSim)
       
       % ADD MODEL TIME AVERAGING  
       filename = fullfile(dirSimData,D(iSim(i)).name);
       bOut = sim == iSim(i); % index for placing data subset
       iSub = iRel(bSurvey & I_SIM==iSim(i)); % index for pulling data subset
       % Load time-evolving data
       u = ncread(filename,'u'); % @ centers
       v = ncread(filename,'v'); % @ centers      
       UXS(:,:,bOut) = u(:,:,iSub);
       UYS(:,:,bOut) = v(:,:,iSub); 
       % ww = ncread(filename,'ww'); % upward water velocity. 
       
       % Interpolate zeta to center points
       zeta = ncread(filename,'zeta');  % zeta is water level at nodes. 
       t_sim = T_SIM(I_SIM==iSim(i));
       X_L = repmat(xn_l,1,size(zeta,2));
       Y_L = repmat(yn_l,1,size(zeta,2));
       T_L = repmat(t_sim',length(xn_l),1);
       tRef = T_L(1);
       T_L = seconds(T_L-tRef);
       G_zeta = scatteredInterpolant(double(X_L(:)),double(Y_L(:)),T_L(:),double(zeta(:)));
       XC_L = repmat(xc_l,1,length(t_sim)); % Create query set at centers
       YC_L = repmat(yc_l,1,length(t_sim));
       TC_L = repmat(t_sim',length(xc_l),1);
       TC_L = seconds(TC_L-tRef);
       
       % Compute zeta
       zeta_c = G_zeta(double(XC_L(:)), double(YC_L(:)), double(TC_L(:)));
       zeta_c = reshape(zeta_c,size(XC_L));

       %d_n = permute(zeta+h_n,[1,3,2]); % time-varying water depth
       %z_n = -d_n.*repmat(siglay_n,1,1,size(d_n,3)); % time-varying nodal depth (positive down, surface relative)
       
       d_c = permute(zeta_c+h_c,[1,3,2]); 
       Z_C = -d_c.*repmat(siglay_c,1,1,size(d_c,3));    
       D_C = repmat(d_c,1,size(XS,2),1);
       % Grab subset (in time) associated with this survey
       ZS(:,:,bOut) = Z_C(:,:,iSub);
       DS(:,:,bOut) = D_C(:,:,iSub);
    end
  
    % Clear some memory before proceeding
    clear zeta_c Z_C XC_L YC_L TC_L X_L Y_L G_zeta u v zeta 
    c1 = toc(tstart);
    fprintf(' completed. Elapsed time: %0.1f sec. (once per survey) \n', c1);
    
    % Copy top layer to zero depth
    XS = cat(2,XS(:,1,:), XS);
    YS = cat(2,YS(:,1,:), YS);
    ZS = cat(2,-0.01+0*ZS(:,1,:), ZS);
    UXS = cat(2, UXS(:,1,:), UXS);
    UYS = cat(2, UYS(:,1,:), UYS);
    
    % Convert data to double: this is data covering entire survey. 
    XS = double(XS);
    YS = double(YS);
    ZS = double(ZS);
    UXS = double(UXS);
    UYS = double(UYS);
    
    % Time-varying depth
    tRef = ts(1);
    xD = squeeze(XS(:,1,:)); 
    yD = squeeze(YS(:,1,:));
    tD = repmat(seconds(ts'-tRef), size(xD,1), 1);
    vD = double(squeeze(DS(:,1,:)));
    fprintf(' Creating time-varying depth function...');
    G_D = scatteredInterpolant(xD(:),yD(:),tD(:),vD(:),'linear','none');
    fprintf('Complete.');
    bSub = SK.testID(:,1) == iSurvey;
    temp = G_D(SK.X(bSub,1), SK.Y(bSub,1), seconds(SK.TS(bSub,1)-tRef));
    SK.DQ(bSub,:) = repmat(temp, 1, size(SK.D,2));

    bSub = SWIFT.testID(:,1) == iSurvey;
    temp = G_D(SWIFT.X(bSub), SWIFT.Y(bSub), seconds(SWIFT.TS(bSub)-tRef));
    SWIFT.DQ(bSub,:) = repmat(temp, 1, size(SWIFT.D,2)); 
    
%%% ~~~~~~~ MODEL AVERAGING ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    dt_model_average = 5; % [minutes]  [0.5 (full res), 1, 2, 5] 
    n_files_averaged = round(dt_model_average./0.5);
    % Average the time-varying data
    % Could do moving mean: x = movmean(UXS, [0,n_files_averaged-1],3);
    t_avg = (ts(1):minutes(dt_model_average):ts(end))';
    x_temp = nan*XS(:,:,1:length(t_avg));
    y_temp = x_temp; z_temp = x_temp; ux_temp = x_temp; uy_temp = x_temp;
    t_temp = TS(:,:,1:length(t_avg));
    for k = 1:length(t_avg)
        i1 = (k-1)*n_files_averaged + 1;
        i2 = i1+n_files_averaged-1;
        if i2 > length(ts)
            i2 = length(ts);
        end
        x_temp(:,:,k) = nanmean(XS(:,:,i1:i2),3);
        y_temp(:,:,k) = nanmean(YS(:,:,i1:i2),3);
        z_temp(:,:,k) = nanmean(ZS(:,:,i1:i2),3);
        ux_temp(:,:,k) = nanmean(UXS(:,:,i1:i2),3);
        uy_temp(:,:,k) = nanmean(UYS(:,:,i1:i2),3);
        % TIMES ARE CENTERED ON AVERAGED INTERVAL
        t_temp(:,:,k) = TS(:,:,i1)+minutes(dt_model_average)./2;     
    end
    
    XS = x_temp;
    YS = y_temp;
    ZS = z_temp;
    UXS = ux_temp;
    UYS = uy_temp;
    TS = t_temp;
    ts_avg = squeeze(TS(1,1,:));

%%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    fprintf(' >> Initializing interpolation functions... ');
    tstart = tic;
    % Initialize interpolation functions 
    X = XS(:,:,1);
    Y = YS(:,:,1);
    Z = ZS(:,:,1);
    U = UXS(:,:,1);
    
    % Step 1 - UX interp function
    GUX1 = scatteredInterpolant(X(:),Y(:),Z(:),U(:),'linear','none'); 
    % Step 1 - UY interp function
    GUY1 = GUX1;
    GUY1.Values = reshape(UYS(:,:,1),[],1);
    
    % If full-resolution processing, use temporal interpolation. 
    if dt_model_average == 0.5
         % Step 2 - UX interp function
        GUX2 = GUX1; 
        GUX2.Points(:,3) = reshape(ZS(:,:,2),[],1); % Update Z coord
        GUX2.Values = reshape(UXS(:,:,2),[],1);
         % Step 2 - UY interp function
        GUY2 = GUX2; 
        GUY2.Values = reshape(UYS(:,:,2),[],1);
    end
        
    c1 = toc(tstart);
    fprintf(' completed. Elapsed time: %0.1f sec. \n', c1);

    %% Assemble query points
    
    % UPDATE SECTION FOR MODEL AVERAGING
    
    for i = 1:length(ts_avg)-1
        tstart = tic;
        %if mod(i,5)==0
        fprintf('  > Interpolating data at timestep %i of %i... \n',i, length(ts_avg)-1);
        % end

        % Define time step
        t1 = ts_avg(i) - minutes(dt_model_average)./2;
        t2 = ts_avg(i+1) - minutes(dt_model_average)./2;
        
        % Get corresponding query pointa            
        xq = []; yq = []; zq = []; tq = []; nq = []; iq = [];
        for k = 1:20
            switch k
                case 1
                    dev = SWIFT;
                case 2
                    dev = SWIFT_60;
                case 3
                    dev = SWIFT_2m;
                case 4
                    dev = SK;
                case 5
                    dev = SK_30;
                case 6
                    dev = SK_60;
                case 7 
                    dev = SK_2m;
                case 8
                    dev = SK_5m;
                case 9
                    dev = FS(iSurvey);
                case 10
                    dev = FS_30(iSurvey);
                case 11
                    dev = FS_60(iSurvey);
                case 12 
                    dev = FS_2m(iSurvey);
            end

           
            bQ = false(size(dev.X));
            bV = ~isnan(dev.X) & ~isnan(dev.Y) & ~isnan(dev.Z);
            bQ(dev.TS >= t1 & dev.TS < t2 & bV) = 1;
            xq = [xq; dev.X(bQ)];
            yq = [yq; dev.Y(bQ)];
            zq = [zq; dev.Z(bQ)];
            tq = [tq; dev.TS(bQ)]; 
            iq = [iq; find(bQ)]; % output index
            if k >= 9 && sum(bQ(:)) > 0
                tbs = [];
            end
            nq = [nq; k+zeros(size(dev.X(bQ)))];  
        end
            
        if ~isempty(xq)    
        % run interpolations
        % Takes about 0.1 second to evaluate. Most of the time is spent
        % updating the interpolation function (~5 sec),
        % so not breaking out timing. 
        % fprintf('  > Running intepolations ...'); 
        uxq = GUX1(xq, yq, zq);       
        uyq = GUY1(xq, yq, zq);
        
        if dt_model_average == 0.5
        uxq2 = GUX2(xq, yq, zq);
        uyq2 = GUY2(xq, yq, zq);

        % compute time weighting
        t_rel = seconds(tq-t1)./seconds(t2-t1);
        uxq = uxq+t_rel.*(uxq2-uxq);
        uyq = uyq+t_rel.*(uyq2-uyq);
        end
 
        % Redistribute output
        for k = 1:20
            iQ = iq(nq == k);
            switch k
                case 1
                    SWIFT.UXQ(iQ) = uxq(nq == k);
                    SWIFT.UYQ(iQ) = uyq(nq == k);
                case 2
                    SWIFT_60.UXQ(iQ) = uxq(nq == k);
                    SWIFT_60.UYQ(iQ) = uyq(nq == k);
                case 3
                    SWIFT_2m.UXQ(iQ) = uxq(nq == k);
                    SWIFT_2m.UYQ(iQ) = uyq(nq == k);
                case 4
                    SK.UXQ(iQ) = uxq(nq == k);
                    SK.UYQ(iQ) = uyq(nq == k);
                case 5
                    SK_30.UXQ(iQ) = uxq(nq == k);
                    SK_30.UYQ(iQ) = uyq(nq == k);
                case 6
                    SK_60.UXQ(iQ) = uxq(nq == k);
                    SK_60.UYQ(iQ) = uyq(nq == k);
                case 7 
                    SK_2m.UXQ(iQ) = uxq(nq == k);
                    SK_2m.UYQ(iQ) = uyq(nq == k);
                case 8
                    SK_5m.UXQ(iQ) = uxq(nq == k);
                    SK_5m.UYQ(iQ) = uyq(nq == k);
                case 9
                    FS(iSurvey).UXQ(iQ) = uxq(nq == k);
                    FS(iSurvey).UYQ(iQ) = uyq(nq == k);
                case 10
                    FS_30(iSurvey).UXQ(iQ) = uxq(nq == k);
                    FS_30(iSurvey).UYQ(iQ) = uyq(nq == k);
                case 11
                    FS_60(iSurvey).UXQ(iQ) = uxq(nq == k);
                    FS_60(iSurvey).UYQ(iQ) = uyq(nq == k);
                case 12 
                    FS_2m(iSurvey).UXQ(iQ) = uxq(nq == k);
                    FS_2m(iSurvey).UYQ(iQ) = uyq(nq == k);
            end 
        end

        end
        
        
        % Update interpolation functions
        if i<length(ts_avg)-1 
            % fprintf('  > Updating interpolation functions...');
           
            if dt_model_average > 0.5
                GUX1.Points(:,3) = reshape(ZS(:,:,i+1),[],1); % Update Z coord
                GUX1.Values = reshape(UXS(:,:,i+1),[],1);
                GUY1 = GUX1;
                GUY1.Values = reshape(UYS(:,:,i+1),[],1);
            else
                % Step 2 now assigned to Step 1
                GUX1 = GUX2;
                GUY1 = GUY2;
                % Update Step 2
                % Note that since we've started on index i = 1, the next+1
                % index is forward 2 time steps from the current index. 
                GUX2.Points(:,3) = reshape(ZS(:,:,i+2),[],1); % Update Z coord
                GUX2.Values = reshape(UXS(:,:,i+2),[],1);
                % Step 2 - UY interp function
                GUY2 = GUX2;
                GUY2.Values = reshape(UYS(:,:,i+2),[],1);
            end
        end
        c1 = toc(tstart);
        fprintf('    completed (%0.1f sec) \n',c1);
        
    end
    c2 = toc(cSurvey);
    fprintf(' >> SURVEY %i COMPLETE. Total elapses time %0.1f min. \n', iSurvey, c2/60);
        
end % End loop of surveys    

%% SAVE THE DATA
fname = sprintf('sim_estimates_model_%s_%dmin_sensor_avg', model_flag, floor(dt_model_average(1)));

fprintf('Saving %s...', fname);% Save the data
save( fname,...
    'FS','FS_30','FS_60','FS_2m',...
    'SK','SK_30','SK_60','SK_2m','SK_5m',...
    'SWIFT','SWIFT_60','SWIFT_2m',...
    'ts_avg');
