%% Update background subtraction mask

clear all;close all;clc

% parpool('local');

se = strel('disk',7);

background_masks_path = '/home/data/matlab_rosado_full_p2_1/background_masks/wide';
original_images_path = '/home/data/matlab_rosado_full_p2_1/wide_left';
disparity_images_path = '/home/data/matlab_rosado_full_p2_1/image_disparity/wide';
corrected_masks_path = '/home/data/matlab_rosado_full_p2_1/background_masks/wide_corrected';

parfor i=0:5142
    image_name = sprintf('%06d.png',i);
    disparity_path = fullfile(disparity_images_path,image_name);
    background_path = fullfile(background_masks_path,image_name);
    corrected_mask_path = fullfile(corrected_masks_path,image_name);
    
    disparity = imread(disparity_path);
    
    disparity_mask = disparity;
    disparity_mask(disparity_mask<50)=0;
    disparity_mask = logical(disparity_mask);

    background_mask = imread(background_path);
    background_mask = im2bw(background_mask,0.1);
    
    background_mask = medfilt2(background_mask, [8 8]);
    background_mask = imclose(background_mask,se);
    background_mask = background_mask & disparity_mask;
    mask = imfill(background_mask,'holes');
    
    imwrite(mask,corrected_mask_path,'PNG');
end

%%

clear all;close all;clc

background_masks_path = '/home/data/matlab_rosado_full_p2_1/background_masks/wide';
original_images_path = '/home/data/matlab_rosado_full_p2_1/wide_left';
disparity_images_path = '/home/data/matlab_rosado_full_p2_1/image_disparity/wide';
corrected_masks_path = '/home/data/matlab_rosado_full_p2_1/background_masks/wide_corrected';

image_name = sprintf('%06d.png',180);
image_path = fullfile(original_images_path,image_name);
disparity_path = fullfile(disparity_images_path,image_name);
background_path = fullfile(background_masks_path,image_name);
corrected_mask_path = fullfile(corrected_masks_path,image_name);

image = imread(image_path);
% image = imresize(image,0.5);

disparity = imread(disparity_path);
% disparity = imresize(disparity,0.5);

disparity_mask = disparity;
disparity_mask(disparity_mask<50)=0;
disparity_mask = logical(disparity_mask);

background_mask = imread(background_path);
% background_mask = imresize(background_mask,0.5);
background_mask = im2bw(background_mask,0.1);

figure('name','Input','numbertitle','off')
imshow(image)


figure('name','Background mask original','numbertitle','off')
imshow(background_mask)

background_mask = medfilt2(background_mask, [8 8]);

se = strel('disk',7);
background_mask = imclose(background_mask,se);

figure('name','Background mask processed','numbertitle','off')
imshow(background_mask)

figure('name','Background and disparity mask','numbertitle','off')
background_mask = background_mask & disparity_mask;
imshow(background_mask)


% figure('name','Backgroun mask','numbertitle','off')
% imshow(background_mask)

% canny_image = edge(rgb2gray(image),'canny',0.1);
 

% background_mask = background_mask | canny_image;

mask = imfill(background_mask,'holes');

% se = strel('disk',1);
% mask = imerode(mask,se);

figure('name','Final mask','numbertitle','off')
imshow(mask)

% se = strel('disk',2);
% background_mask = imopen(background_mask,se);

r = image(:,:,1);
r(mask) = 255;
image(:,:,1)=r;
image_mask = image;
image_mask(:,:,1) = r;



figure('name','final','numbertitle','off')
imshow(image_mask)


%% Segment the pedestrian

clear all;close all;clc

image_name = '000050.png';
image_path = fullfile('/home/data/matlab_rosado_full_p2_1/wide_left',image_name);
disparity_path = fullfile('/home/data/matlab_rosado_full_p2_1/image_disparity/wide',image_name);
background_path = fullfile('/home/data/matlab_rosado_full_p2_1/background_masks/wide',image_name);

image = imread(image_path);
image = imresize(image,0.5);

disparity = imread(disparity_path);
disparity = imresize(disparity,0.5);

disparity_mask = disparity;
disparity_mask(disparity_mask<50)=0;
disparity_mask = logical(disparity_mask);

background_mask = imread(background_path);
background_mask = imresize(background_mask,0.5);
background_mask = im2bw(background_mask,0.1);

figure('name','Input','numbertitle','off')
imshow(image)

figure('name','Backgroun mask','numbertitle','off')
imshow(background_mask)

figure('name','Background and disparity mask','numbertitle','off')
mask = background_mask & disparity_mask;
imshow(mask)

canny_image = edge(rgb2gray(image),'canny',0.1);
 

% cform = makecform('srgb2lab');
% lab_he = applycform(image,cform);
% 
% ab = double(lab_he(:,:,2:3));
% nrows = size(ab,1);
% ncols = size(ab,2);
% ab = reshape(ab,nrows*ncols,2);
% 
% nColors = 10;
% % repeat the clustering 3 times to avoid local minima
% [cluster_idx, cluster_center] = kmeans(ab,nColors,'distance','sqEuclidean','Replicates',3);
% 
% pixel_labels = reshape(cluster_idx,nrows,ncols);
% figure('name','k means','numbertitle','off')
% imshow(pixel_labels,[]), title('image labeled by cluster index');
% 
% 
% canny_kmeans = edge(pixel_labels,'canny',0.1);
% 
% se = strel('disk',1);
% canny_kmeans = imclose(canny_kmeans,se);
% 
% 
% % se = strel('disk',2);
% % canny_kmeans = imclose(canny_image,se);
% 
% figure('name','canny_kmeans','numbertitle','off')
% imshow(canny_kmeans)
% 
% canny_back_disp = (canny_kmeans & disparity_mask) | mask;
% 
% figure('name','canny_back_disp','numbertitle','off')
% imshow(canny_back_disp)
% 
% canny_back_disp_holes = imfill(canny_back_disp,'holes');
% 
% figure('name','canny_back_disp_holes','numbertitle','off')
% imshow(canny_back_disp_holes)

se = strel('disk',3);
mask = imdilate(mask,se);

r = image(:,:,1);
r(mask) = 255;
image(:,:,1)=r;
image_mask = image;
image_mask(:,:,1) = r;


figure('name','image_mask','numbertitle','off')
imshow(image_mask)


%%


% canny_image = edge(rgb2gray(image),'canny',0.01);

cform = makecform('srgb2lab');
lab_he = applycform(image,cform);

ab = double(lab_he(:,:,2:3));
nrows = size(ab,1);
ncols = size(ab,2);
ab = reshape(ab,nrows*ncols,2);

nColors = 2;
% repeat the clustering 3 times to avoid local minima
[cluster_idx, cluster_center] = kmeans(ab,nColors,'distance','sqEuclidean','Replicates',3);

pixel_labels = reshape(cluster_idx,nrows,ncols);
figure('name','k means','numbertitle','off')
imshow(pixel_labels,[]), title('image labeled by cluster index');

canny_kmeans = edge(pixel_labels,'canny',0.1);

figure('name','Canny image','numbertitle','off')
imshow(canny_kmeans)
truesize

canny_disparity = edge(disparity,'canny',0.3);

figure('name','Canny disparity','numbertitle','off')
imshow(canny_disparity)
truesize

canny_composed = (canny_kmeans ) & disparity_mask;

figure('name','Canny composed','numbertitle','off')
imshow(canny_composed)
truesize

[imlabel, totalLabels] = bwlabel(canny_composed,4);
sizeBlob = zeros(1,totalLabels);


% [maxno, largestBlobNo] = max(sizeBlob);

% imshow(disparity_mask)

%%

figure('name','Input','numbertitle','off')
imshow(image)

cform = makecform('srgb2lab');
lab_he = applycform(image,cform);

ab = double(lab_he(:,:,2:3));
nrows = size(ab,1);
ncols = size(ab,2);
ab = reshape(ab,nrows*ncols,2);

nColors = 3;
% repeat the clustering 3 times to avoid local minima
[cluster_idx, cluster_center] = kmeans(ab,nColors,'distance','sqEuclidean','Replicates',3);

pixel_labels = reshape(cluster_idx,nrows,ncols);
figure('name','k means','numbertitle','off')
imshow(pixel_labels,[]), title('image labeled by cluster index');

% 
% figure
% imshow(disparity)
% 
canny_disp = edge(disparity,'canny',0.1);
canny_disp(disparity_mask==0) = 0;
canny_kmeans = edge(pixel_labels,'canny',0.1);
canny_kmeans(disparity_mask==0) = 0;
% 
figure('name','Canny disparity','numbertitle','off')
imshow(canny_disp)
figure('name','Canny kmeans','numbertitle','off')
imshow(canny_kmeans)

se = strel('disk',2);
canny_kmeans = imclose(canny_kmeans,se);

edge_composed = canny_disp | canny_kmeans;

figure('name','Canny composed','numbertitle','off')
imshow(edge_composed)

% background_mask = bwareaopen(background_mask, 20);

se = strel('disk',3);
background_mask = imerode(background_mask,se);


figure('name','Background mask','numbertitle','off')
imshow(background_mask)

edge_background_composed = imfill(edge_composed,find(background_mask));

figure('name','Background composed','numbertitle','off')
imshow(edge_background_composed)

%
% close all


% disparity_mask = xor(disparity_mask,canny_kmeans);
figure('name','Disparity with max','numbertitle','off')
imshow(disparity_mask)

edge_background_max = edge_background_composed & disparity_mask;

figure('name','Edge with background and max','numbertitle','off')
imshow(edge_background_max)

[imlabel, totalLabels] = bwlabel(edge_background_max,4);
sizeBlob = zeros(1,totalLabels);
for i=1:totalLabels,
    sizeBlob(i) = length(find(imlabel==i));
end
[maxno, largestBlobNo] = max(sizeBlob);

person = zeros(size(edge_background_max),'uint8');
person(imlabel==largestBlobNo) = 1;
person = logical(person);

figure('name','Largest blog','numbertitle','off')
imshow(person)


%% Aggregate Channel Features detector example

clear all;close all;clc

addpath(genpath('piotr_toolbox'));

load AcfInriaDetector.mat

figure('name','ACF detector','numbertitle','off')

for i=0:1000
    image_name = sprintf('%06d.png',i);
    image_path = fullfile('/home/data/matlab_rosado_full_p2_1/wide_left',image_name);

    bbs = acfDetect(image_path,detector);

    if size(bbs,1)>0
        bbs = bbNms(bbs, 'type','max' );
        bbs = bbs(1,:);%Get only the top detection

        imshow(image_path)
        hs = bbApply( 'draw',bbs);
        set(findobj(hs,'Type','text'),'color',[1 0 0])
    else
        imshow(image_path)
        text(10,10,'No detection','color',[1 0 0])
    end
    
%     pause(0.01)
    drawnow
end

%% Pre crop positives
close all;clear all;clc

crop = CropPositives('/home/data/matlab_rosado_full_p2_1/wide_left',...
    '/home/data/matlab_rosado_full_p2_1/wide_left_croped_pos');

%% Train a detector
close all;clear all;clc

dataDir = '/home/data/matlab_rosado_full_p2_1';

opts=acfTrain(); 
opts.modelDs=[100 41]; 
opts.modelDsPad=[128 64];
opts.nWeak=[32 128 512 2048];
opts.pJitter=struct('flip',0);
opts.pBoost.pTree.fracFtrs=1/16;
opts.pLoad={'squarify',{3,.41}}; 
opts.name='models/EssuaRosado';

opts.posWinDir=fullfile(dataDir,'wide_left_croped_pos');
opts.posImgDir=fullfile(dataDir,'wide_left');
opts.negImgDir=fullfile(dataDir,'wide_left_neg');


detector = acfTrain( opts );

% opts=acfTrain(); opts.modelDs=[100 41]; opts.modelDsPad=[128 64];
% opts.posGtDir=[dataDir 'train/posGt']; opts.nWeak=[32 128 512 2048];
% opts.posImgDir=[dataDir 'train/pos']; opts.pJitter=struct('flip',1);
% opts.negImgDir=[dataDir 'train/neg']; opts.pBoost.pTree.fracFtrs=1/16;
% opts.pLoad={'squarify',{3,.41}}; opts.name='models/AcfInria';

%% Grabcut

clear all;close all;clc

% cd SegTool/SegToolBox

addpath SegTool
% SegTool


% cd SegTool/
% mex GraphCutSegment.cpp graph.cpp maxflow.cpp 
% mex GraphCutSegmentLazy.cpp graph.cpp maxflow.cpp
% cd ..

im = '/home/data/matlab_rosado_full_p2_1/wide_left/000061.png';

% ih = imshow(im);

SmartSelectSeg

% % Global Variables
% global sopt;
% 
% % Initialize all required parameters
% sopt = mksopt;
% 
% SegmentGC(im,ih,0)