%% Wave Quantification Example
% Sharon Kim, Application Engineer
% This script explores different methods for quantifying and localizing waves approaching a buoy in radar data.
% Table of Contents
% Data Pre-Processing
%     Load Data
%         Visualize Data as Montage
%         Rotate images to desired orientation
%     Find lines (wave fronts)
%         Use Hough to find lines in image
%         Farneback Optical Flow
%         Visualize Optical Flow Results
%     Use Projected Signal
%         Visualize Wave Shifts per Frame
%         Other Methods to Consider
%     Use Live Tasks for Method Prototyping
%
%% Data Pre-Processing
%% Load Data

load TenScansAll222253.mat
close all % Close any figures 
clearvars -except VBsave % Retain just VBsave data

%
%% Visualize Data as Montage

figure; montage(VBsave,'Size',[2 inf])

%% Rotate images to desired orientation

VBsave = cellfun(@(x) fliplr(imrotate(x,180)),VBsave,'UniformOutput',false);
montage(VBsave,'Size',[2 inf])

%
%% Find lines (wave fronts)
%% Use Hough to find lines in image

num = 2; % Choose which image to explore
img = VBsave{num};

% Parameters / Code originally provided by Rob
waveHeading = 66;  exclusion = 5; thetaExcl= 2*exclusion-1; nPeaks = 7; 
[H, T, R] = hough(img,'theta', waveHeading-exclusion:waveHeading+exclusion); 
P  = houghpeaks(H,nPeaks,'theta',T,'nhoodsize',[91 thetaExcl]);

% Find lines
lines = houghlines(img,T,R,P,'FillGap',150,'MinLength',500);

% Plot lines on image
figure, imshow(img), hold on; 
for k = 1:length(lines)
   xy = [lines(k).point1; lines(k).point2];
   plot(xy(:,1),xy(:,2),'LineWidth',2,'Color','green');
   % Plot beginnings and ends of lines
   plot(xy(1,1),xy(1,2),'x','LineWidth',2,'Color','yellow');
   plot(xy(2,1),xy(2,2),'x','LineWidth',2,'Color','red');
end

%
%% Farneback Optical Flow
% In the case of the radar data we are working with, the Farneback method is most effective for obtaining optical flow information. (In cases where data collection is more frequent or apparent motion is slower, other optical flow methods could apply.)

testFlow = cell(size(VBsave)); % Preallocate variable to save flow calculations

opticFlow = opticalFlowFarneback( ...
    "FilterSize",90, ... 
    "NeighborhoodSize", 6);

opticFlow.reset % reset system object 
for i=1:length(VBsave)
    testFlow{i} = estimateFlow(opticFlow,VBsave{i});  
end
disp('Optical Flow estimation calculated.')

%
%% Visualize Optical Flow Results

num = 2; % Choose which image to explore
imshowpair(imfill(VBsave{num-1},'holes'),imfill(VBsave{num},'holes'));

hold on;
    plot(testFlow{i},'DecimationFactor',[1 1].*25, ...
        'ScaleFactor',12,'Parent',gca);
    plot(0,0,'g-'); plot(0,0,'m-'); % Set legend colors
hold off;
lgd = legend('Flow','Previous','Current');

%
%% Use Projected Signal
% Let's try rotating our image such that the wave fronts are organized vertically, and take a projected sum to identify wave front locations through signal analysis.

num = 4; % Choose which image to explore
img = VBsave{num};
img = imfill(img,'holes');
img = imrotate(img,66); % Rotate such that wavefronts are roughly vertical
imshow(img); title('Rotated Image')

sigsum = sum(img,1);
sigsum = smoothdata(sigsum,'gaussian','SmoothingFactor',0.25);
sigsum = detrend(sigsum,2);
sigsum = normalize(sigsum,'range');
plot(sigsum)
title('Normalized Projected Signal Magnitude');

%
%% Visualize Wave Shifts per Frame
% Let's visualize our waves, detecting them as peaks in our projected signal, and also running cross correlation analysis for automated lag estimation between frames. (Note that we can also automate this process by choosing to analyze the location of only the peaks approaching the center.)

num = 4; % Set total number of frames we want to analyze here
clf; figure; cm = (hsv(num));
for i = 1:num    
    % Process image - rotation, sum, smoothing, detrending, normalization
    img = VBsave{i};
    sigsum = sum(imrotate(imfill(img,'holes'),66),1); 
    sigsum = smoothdata(sigsum,'gaussian','SmoothingFactor',0.25);
    sigsum = detrend(sigsum,2);
    sigsum = normalize(sigsum,'range');
    hold on; plot( sigsum, 'Color', cm(i,:),'DisplayName','Summed Projection Signal'); hold off;
    
    if i > 1 % For second frame on, calculate cross correlation with previous signal
        [xcf,lags] = xcorr(prevsigsum,sigsum);
        [M,ind] = max(xcf); 
        disp("Cross Correlation peak lag for " + i + " is : " + lags(ind) + " m.");
    end
    
    hold on;
    maxIndices = islocalmax(sigsum,'MinProminence',0.15,'MaxNumExtrema',15);
    plot(find(maxIndices),sigsum(maxIndices),'^','Color',cm(i,:),...
    'MarkerFaceColor',cm(i,:),'DisplayName','Local maxima');
    stem(find(maxIndices),ones(size(find(maxIndices))),'Color',cm(i,:),'DisplayName','Local Maxima Index');
    text(find(maxIndices),ones(size(find(maxIndices))).*1.1,num2str(i),'Color',cm(i,:));
    hold off;
    
    prevsigsum = sigsum; 
end
hold on; plot([501 501],[-.2 1.2],'k--','DisplayName','Center'); hold off;
xlim([125 1200]); ylim([-.2 1.2]); lg = legend; lg.Location = 'eastoutside';

ylabel('Normalized Projected Signal Magnitude');
xlabel('Distance (m)')

%
%% Other Methods to Consider
% Clustering Methods
% Multiple Object Tracking - vision.ForegroundDetector, custom Cascade Object Detector, vision.PointTracker (KLT)
% Time-Frequency Analysis - including Wavelet Coherence Analysis
%
%% Use Live Tasks for Method Prototyping
% We can leverage live tasks to quickly prototype methods for tasks including smoothing data, detrending ddata, or finding peaks in data. Below is an example of such a live task for
%

% Find local maxima
maxIndices = islocalmax(sigsum,'MinProminence',0.15,'MaxNumExtrema',15);

% Display results
clf
plot(sigsum,'Color',[109 185 226]/255,'DisplayName','Input data')
hold on

% Plot local maxima
plot(find(maxIndices),sigsum(maxIndices),'^','Color',[217 83 25]/255,...
    'MarkerFaceColor',[217 83 25]/255,'DisplayName','Local maxima')
title(['Number of extrema: ' num2str(nnz(maxIndices))])
hold off
legend