% Analyze old version VR Cine 436x512 in sag view
% Add enclosed line on the top and bottom
% Crop target
% detect contours
% remove conoturs
% detect the crosslines in Cine and use mask to get PCC
% detect the motion
%
% Weihua Mao @ Henry Ford Health System,  Aug 2022

clear all;
tic;
MRN = 'Research03';
View = 'Ax';
fn_results = [MRN '\' MRN '_results.txt'];
         
filename_video = [MRN '\' MRN '_' View '.mp4'];
%filename_save_mat = [MRN '\' MRN '_Crossline_3F2.mat'];

color_min = 128;
color_dif = 50;
Color_subtract = 50;
bkg = 5;
Nw = 250; % skull range
DN = 100; % range to calc gradient
dn = 5;  % range to detect cross line
din = 5; %cross line width: +/-din
DNy1 = 48;
DNy2 = 35;
DNx1 = 15;
DNx2 = 15;
se8 = strel('disk', 8);

CL_x = 240;
CL_y = 200;
Ny0 = 428;

% optimization inital
rot0 = 0;
x0 = 0;
y0 = 0;        

% optimization range
dx0 = 0.5; % 0.2 mm
dy0 = dx0;
dtheta0 = 0.25 * asin(1.0)/90.0; % 0.2 Degrees
step_ratio = 3; 
nstep = 3; %steps: 1, 3, 9; ranges: +/- 3, 9, 27 (12mm, 7Deg)

vidObj = VideoReader(filename_video);
Frame_Rate = vidObj.FrameRate;
n = 0;
while hasFrame(vidObj)
        n = n + 1;   
        vidFrame0 = readFrame(vidObj);
        Vid(:, :, :, n) = vidFrame0(:, :, :);
end
N_frame = n;

[Nv, Nu, n3, N_Frame] = size(Vid);
Nx = Nu - DNx1 - DNx2;
Ny = Nv - DNy1 - DNy2;
if Ny0 >= Ny
        Ny0 = Ny;
end
Nu1 = DNx1 + 1;
Nu2 = Nu - DNx2;
Nv1 = DNy1 + 1;
Nv2 = Nv - DNy2;

Nx1 = round(Nx/8.0, 0);
Nx2 = Nx - Nx1;
Ny1 = round(Ny/8.0, 0);
Ny2 = Ny - Ny1;
formspec = '%10.3e';
Results = zeros(N_frame, 6);

% Display Frame #1
vidFrame0 = uint8(zeros(Nv, Nu, n3));
vidFrame0(:, :, :) = Vid(:, :, :, 1);        
subplot(2, 3, 1),  imshow(vidFrame0); title('Initial');

% Find Target centers

viFrame = uint8(zeros(Ny, Nx, 3));
viFrame(1:Ny, 1:Nx, 1:3) = vidFrame0(Nv1:Nv2, Nu1:Nu2, 1:3);
subplot(2, 3, 2), imshow(viFrame);
contour = getContours(viFrame, color_min, color_dif);
subplot(2, 3, 3), imshow(contour);

close_contour = imdilate(contour, se8);
close_contour = imdilate(close_contour, se8); 
fill_contour = imfill(close_contour, 'holes');
final_contour = imerode(fill_contour, se8);
final_contour = imerode(final_contour, se8);
final_contour = logical(final_contour);
final_contour = bwareafilt(final_contour, 1);              
subplot(2, 3, 3),  imshow(final_contour); title('target');        
Stats = regionprops(final_contour);
Center = Stats.Centroid;
BoundingBox = Stats.BoundingBox;
NC_x = Center(1);
NC_y = Center(2);

% remove contours        
avgFrame = zeros(Ny, Nx);
avgFrame = (double(viFrame(:, :, 1)) + double(viFrame(:, :, 2)) + double(viFrame(:, :, 3)))/3.0;
mask = ones(Ny, Nx) - contour;
maskFrame =  zeros(Ny, Nx);        
maskFrame = avgFrame .* mask; 
counts = zeros(Ny, Nx);
sumFrame = zeros(Ny, Nx);        
for nx = -5 : 5
        for ny = -5 : 5
                counts(Ny1:Ny2, Nx1:Nx2) = counts(Ny1:Ny2, Nx1:Nx2) + mask(Ny1+ny:Ny2+ny, Nx1+nx:Nx2+nx);
                sumFrame(Ny1:Ny2, Nx1:Nx2) = sumFrame(Ny1:Ny2, Nx1:Nx2) + maskFrame(Ny1+ny:Ny2+ny, Nx1+nx:Nx2+nx);
        end
end                        
sumFrame = sumFrame .* (ones(Ny, Nx) - mask); 
zero_counts = zeros(Ny, Nx);
zero_counts(counts < 1) = 1;  
all_counts = zero_counts + counts;
Frame = sumFrame ./ all_counts;
avgFrame0 = Frame  + avgFrame.* mask;
subplot(2, 3, 1),  imshow(avgFrame0); caxis([0, 255]); title('Frame# 1');
sum_0 = sum(avgFrame0(:));
mask_0 = getCrosslineMask(avgFrame0, bkg, CL_x, CL_y, DN, dn, din);
mask_0(Ny0:Ny, :) = 0;

subplot(2, 3, 3),  imshow(mask_0); 
avgFrame_mask = avgFrame0 .* double(mask_0);
subplot(2, 3, 2),  imshow(avgFrame_mask);  caxis([0, 255]); title('Frame# 1');

mask_pixels = sum(mask_0(:));
avgFrame_avg = sum(avgFrame_mask(:)) / mask_pixels;
avgFrame_1 = (avgFrame0 - avgFrame_avg) .* double(mask_0); 
PCC_XX = sum(sum(avgFrame_1 .* avgFrame_1));
[optimizer, metric]  = imregconfig('monomodal');  
tform0 = imregtform(avgFrame0, avgFrame0, 'similarity', optimizer, metric);

for n = 2 : N_frame                  
        figure_save =  [MRN '\jpeg\' MRN '_' num2str(n) '_CL_' View '.jpg'];
        PCC0 = 0;
        vidFrame = uint8(zeros(Nv, Nu, n3));
        vidFrame(:, :, :) = Vid(:, :, :, n);        
        subplot(2, 3, 4),  imshow(vidFrame);

        %remove contours
        viFrame = uint8(zeros(Ny, Nx, 3));
        viFrame(1:Ny, 1:Nx, 1:3) = vidFrame(Nv1:Nv2, Nu1:Nu2, 1:3);
        subplot(2, 3, 5), imshow(viFrame);
        contour = getContours(viFrame, color_min, color_dif);
        subplot(2, 3, 5), imshow(contour);
        Stats = regionprops(contour);
        Center = Stats.Centroid;
        BoundingBox = Stats.BoundingBox;
        NC_x = Center(1);
        NC_y = Center(2);

        viFrame = viFrame;
        avgFrame = zeros(Ny, Nx);
        avgFrame = (double(viFrame(:, :, 1)) + double(viFrame(:, :, 2)) + double(viFrame(:, :, 3)))/3.0;
        mask = ones(Ny, Nx) - contour;
        maskFrame =  zeros(Ny, Nx);        
        maskFrame = avgFrame .* mask; 
        counts = zeros(Ny, Nx);
        sumFrame = zeros(Ny, Nx);        
        for nx = -5 : 5
                for ny = -5 : 5
                        counts(Ny1:Ny2, Nx1:Nx2) = counts(Ny1:Ny2, Nx1:Nx2) + mask(Ny1+ny:Ny2+ny, Nx1+nx:Nx2+nx);
                        sumFrame(Ny1:Ny2, Nx1:Nx2) = sumFrame(Ny1:Ny2, Nx1:Nx2) + maskFrame(Ny1+ny:Ny2+ny, Nx1+nx:Nx2+nx);
                end
        end                        
        sumFrame = sumFrame .* (ones(Ny, Nx) - mask); 
        zero_counts = zeros(Ny, Nx);
        zero_counts(counts < 1) = 1;  
        all_counts = zero_counts + counts;
        Frame = sumFrame ./ all_counts;
        avgFrame = Frame  + avgFrame.* mask;
        avgFrame = avgFrame * sum_0 / sum(avgFrame(:));
        subplot(2, 3, 4), imshow(avgFrame); caxis([0, 255]); title(['Frame# ' num2str(n)]);
        dif = avgFrame/mean(avgFrame(:)) - avgFrame0/mean(avgFrame0(:));
        dif_mask = dif .* mask_0;
        dif_sum = sum(sum(dif_mask .* dif_mask));
        subplot(2, 3, 3),  imshow(dif);  caxis([-0.5 0.5]); title(['Inital Frame # ' num2str(n) ' - Frame # 1 = ' num2str(dif_sum, formspec)]);
        
        for step = 1 : nstep
                n_times = step_ratio^(nstep-step);
                dtheta = dtheta0 * n_times;
                dx = dx0 * n_times;
                dy = dy0 * n_times;
                D_theta = step_ratio * dtheta;
                D_x = step_ratio * dx;
                D_y = step_ratio * dy;                

                theta1 = rot0 - D_theta;
                theta2 = rot0 + D_theta;
                x1 = x0 - D_x;
                x2 = x0 + D_x;
                y1 = y0 - D_y;
                y2 = y0 + D_y;
                tform = tform0;
                for theta = theta1 : dtheta : theta2
                        cos_theta = cos(theta);
                        sin_theta = sin(theta);
                        tform.T(1, 1) = cos_theta;
                        tform.T(1, 2) = -sin_theta;
                        tform.T(2, 2) = cos_theta;
                        tform.T(2, 1) = sin_theta;
                        for x = x1 : dx : x2
                                tform.T(3, 1) = x;
                                for y = y1 : dy : y2
                                        tform.T(3, 2) = y;
                                        New_avgFrame = imwarp(avgFrame,tform,'OutputView',imref2d(size(avgFrame0)));
                                        New_avgFrame_mask = New_avgFrame .* mask_0;
                                        New_avgFrame_avg = sum(New_avgFrame_mask(:)) / mask_pixels;
                                        New_avgFrame_1 = (New_avgFrame - New_avgFrame_avg) .* mask_0; 
                                        PCC_XY = sum(sum(New_avgFrame_1 .* avgFrame_1));
                                        PCC_YY = sum(sum(New_avgFrame_1 .* New_avgFrame_1));
                                        PCC = PCC_XY/sqrt(PCC_YY * PCC_XX);
                                        if PCC > PCC0
                                                PCC0 = PCC;
                                                x0 = x;
                                                y0 = y;
                                                rot0 = theta;
                                                subplot(2, 3, 5),  imshow(New_avgFrame);  caxis([0, 255]); title(['rot= ' num2str(theta) ' dx= ' num2str(x) ' dy= ' num2str(y) ' PCC= ' num2str(PCC)]);
                                                dif = New_avgFrame/mean(New_avgFrame(:)) - avgFrame0/mean(avgFrame0(:));
                                                dif_mask = dif .* mask_0;
                                                dif_sum = sum(sum(dif_mask .* dif_mask));
                                                subplot(2, 3, 6),  imshow(dif);  caxis([-0.5 0.5]); title(['Final Frame # ' num2str(n) ' - Frame # 1 = ' num2str(dif_sum, formspec)]);
                                        end
                                end
                        end
                end
        end
        Results(n, 1) = rot0;
        Results(n, 2) = x0;
        Results(n, 3) = y0;
        Results(n, 4) = PCC0;
        Results(n, 5) = NC_x;
        Results(n, 6) = NC_y;

        saveas(gcf, figure_save, 'jpeg');        

end

fp = fopen(fn_results, 'a');        
for n = 1 : N_frame                  
        fprintf(fp, '%s, %d, %9.5f, %9.5f, %9.5f, %f, %f, %f\n', View, n, Results(n, 1), Results(n, 2), Results(n, 3), Results(n, 4), Results(n, 5), Results(n, 6));
end                
fclose(fp);
toc;
save(filename_save_mat);