clear,close all;
%% import data
v = VideoReader('./data/906-17_l.avi');
video = zeros(v.Height,v.Width,3,v.Duration*v.FrameRate,'uint8'); %preallocate data for video, [height, width, color channels, frames]
i = 1;
while hasFrame(v) % extract all the frames in the video (for surveillance continuous data extraction needs to be applied)
    video(:,:,:,i) = readFrame(v);
%     imshow(video(:,:,:,i));drawnow;
    i = i+1;
end
% figure;colormap jet
%% compute optical flow (Piotr's Master Toolbox)
Vopt = zeros(size(video,1),size(video,2),size(video,4),2); %preallocate optical flow matrix [x-coordinate,y-coordinate,frame,optical flow direction (1 = x, 2 = y)]
for i = 2:size(video,4) %for every frame
    I1 = video(:,:,:,i-1);
    I2 = video(:,:,:,i);
    [Vx,Vy] = opticalFlow(rgb2gray(I1),rgb2gray(I2),'smooth',1,'radius',10,'type','LK'); %use Piotr,s optical flow
    Vopt(:,:,i,1) = Vx;
    Vopt(:,:,i,2) = Vy;
end

%% compute particle advection
[X,Y] = meshgrid(10:10:size(video,1)-10,10:10:size(video,2)-10); %initial particle positions
Opt_av = zeros(size(X,1),size(X,2),2);%preallocating
w_av = fspecial('gaussian',[11,11]); %filter for calculating the weighted average optical flow of a particle

%initialize
X2 = reshape(X,[1,size(X,1)*size(X,2)]);
Y2 = reshape(Y,[1,size(Y,1)*size(Y,2)]);
I = video(:,:,:,1);
figure;
imshow(I);hold on;
scatter(Y2,X2,5,'y','filled');
hold off;
pause
%advection
tic
for i = 2:size(video,4)
    for x = 1:size(X,1) %particle x-coordinate
        for y = 1:size(X,2) %particle y-coordinate
            X_ROI = round([X(x,y)-5:X(x,y)+5]); %determin Region Of Interest
            Y_ROI = round([Y(x,y)-5:Y(x,y)+5]);
            Opt_ROI = Vopt(X_ROI,Y_ROI,2,:); % extract optical flow of ROI, TO DO: fix matrix dimensions exceeding

            Opt_av(x,y,1) = sum(sum(w_av.*Opt_ROI(:,:,1,1))); %calculate weighted average (x-direction)
            Opt_av(x,y,2) = sum(sum(w_av.*Opt_ROI(:,:,1,2))); %calculate weighted average (y-direction)
        end
    end
    %calculate new particle positions
    X = X+Opt_av(:,:,1);
    Y = Y+Opt_av(:,:,2);
    X2 = reshape(X,[1,size(X,1)*size(X,2)]);
    Y2 = reshape(Y,[1,size(Y,1)*size(Y,2)]);
    imshow(video(:,:,:,i)); hold on;
    scatter(Y2,X2,5,'y','filled');
	hold off;
    drawnow;
    pause
end
toc


%% visualize
% [X,Y] = meshgrid(1:5:size(video,2),1:5:size(video,1));
% Vopt_vis = Vopt(1:5:end,1:5:end,:,:); % for visualisation, decrease amount of arrows
% for x = 1:size(Vopt_vis,1)
%     for y = 1:size(Vopt_vis,2)
%         if sqrt(Vopt_vis(x,y,:,1).^2+Vopt_vis(x,y,:,2).^2) < 0.25 %don't display to small arrows
%             Vopt_vis(x,y,:,1) = 0; Vopt_vis(x,y,:,2) = 0;
%         end
%     end
% end
% figure;
% for i = 2:size(Vopt_vis,3)
%     Vx = Vopt_vis(:,:,i-1,1);
%     Vy = Vopt_vis(:,:,i-1,2);
%     
%     I = video(:,:,:,i-1);
%     imshow(I);hold on; %frame
%     quiver(X,Y,Vx,Vy,'AutoScaleFactor',3,'color','blue') %optical flow field
%     drawnow; %forces matlab to plot image before continuing
% end