%% Clear and prepare voc

close all;clear all;clc

release = 4;

if release == 4
    addpath_recurse('../voc-release4.01')

    %cd ../voc-release4.01
    %compile
end

if release == 5
    addpath_recurse('../voc-release5')
    cd ../voc-release5
    
    startup
    cd ../voc-kitti
end

try
%     matlabpool close
%     matlabpool open 4
catch
end

addpath(genpath('../../mexopencv/'))
addpath matpcl

if release~=4
    error('This code is configured to work with release 4 and no other')
end

%% Load model

%load models/pedestrian_hog4d_full_1/Pedestrian_final
load models/pre-trained/pedestrian_2_2012_10_04-20_25_13_model

figure

C=8;
model = modifymodel(model,C);
my_visualizemodel(model,C)
% visualizemodel(model)
%% Select dataset

dataset_folder = '/usr/local/share/Kitti/person/2011_09_28_drive_0053/';
% dataset_folder = '/usr/local/share/Kitti/person/2011_09_28_drive_0104/';
% dataset_folder = '/usr/local/share/Kitti/person/2011_09_28_drive_0078/';
% dataset_folder = '/usr/local/share/Kitti/person/2011_09_28_drive_0183/';
% dataset_folder = '/usr/local/share/Kitti/person/2011_09_28_drive_0199/';
% dataset_folder = '/usr/local/share/Kitti/person/2011_09_28_drive_0208/';
% dataset_folder = '/usr/local/share/Kitti/person/2011_09_28_drive_0216/';

left_color_folder = fullfile(dataset_folder,'/image_02/data');
disparity_filestorage_folder = fullfile(dataset_folder,'/image_disparity_filestorage/data');
pcd_folder = fullfile(dataset_folder,'/pcd/data');
pcd_filtered_folder = fullfile(dataset_folder,'/pcd_filtered/data');
detections_folder = fullfile(dataset_folder,'/detections/data');

mkdir(detections_folder);


%% Get detections for all images in dataset (save .mat files)

left_color_files = dir(fullfile(left_color_folder,'*.png'));

for i=1:size(left_color_files,1)
    
    name = left_color_files(i).name(1:end-4);
    
    %Get color image path
    left_color_path = fullfile(left_color_folder,[name '.png']);
    
    %Get detections file path
    detections_path = fullfile(detections_folder,name);
    
    %Load left color image
    left_color = im2double(imread(left_color_path));
    
    fprintf('processing ...%s,  ',left_color_path(end-20:end));
    [bbox, fbbox] = process(left_color, model, 0.0);
    fprintf('%d detections\n',size(bbox,1));
    
    save(detections_path,'bbox','fbbox');
end

%% Get 3d pose of points, use pcd filtered point cloud, plot 3D
close all;

figure('position',[50 50 800 800])
set(gca, 'LooseInset', [0 0 0 0] )

hold on

left_color_files = dir(fullfile(left_color_folder,'*.png'));

T1=rotation_angle_axis(deg2rad(-90),[0 0 1]);
T2=rotation_angle_axis(deg2rad(-10),[0 1 0]);
rotation = T2*T1;

% for i=1:size(left_color_files,1)
for i=2:size(left_color_files,1)
    name = left_color_files(i).name(1:end-4);
    
    %Get detections file path
    detections_path = fullfile(detections_folder,name);
    
    %Get filtered pcd file path
    pcd_filtered_path = fullfile(pcd_filtered_folder,[name '.pcd']);
    
    %Load filtered pcd path
    pcd = loadpcd(pcd_filtered_path);
    
    %Load detections
    load(detections_path);
    
    %Get positions of the detections
    centers = bboxcenterpos([bbox(1,1:4) fbbox(1,:)]);
    centers(1,:)=[];
    
    centers=round(centers);
    
    x=pcd(:,:,1);
    y=pcd(:,:,2);
    z=pcd(:,:,3);
    r=pcd(:,:,4);
    g=pcd(:,:,5);
    b=pcd(:,:,6);
    
    colors=[r(:)';g(:)';b(:)'];
    P=[x(:)';y(:)';z(:)'];
    
    invalidInf = isinf(P(1,:));
    invalidNaN = isnan(P(1,:));
    
    invalid = logical(invalidInf + invalidNaN);
    
    P(:,invalid)=[];
    colors(:,invalid)=[];
    
    Pr=rotation*P;
    
    clf
    hold on
    
    scatter3(Pr(1,:),Pr(2,:),Pr(3,:),12*ones(1,size(Pr,2)),colors','filled')
    
    cp=[14 1.5 0.0];
    dv=2;
    
    axis([cp(1)-dv cp(1)+dv cp(2)-dv cp(2)+dv cp(3)-dv cp(3)+dv])
    
    axis square
    grid on

    xlabel('x')
    ylabel('y')
    zlabel('z')
    
    view(-50,10);
    
    for d=1:size(centers,1)
        
        x = pcd(centers(d,2),centers(d,1),1);
        y = pcd(centers(d,2),centers(d,1),2);
        z = pcd(centers(d,2),centers(d,1),3);
        
        p = rotation*[x;y;z];
        
        plot3(p(1),p(2),p(3),'o','MarkerFaceColor',[1 0 0],'MarkerSize',12);
    end
    
    pause(1)
    
end

%% Plot detections and obtain 3d information

figure

left_color_files = dir(fullfile(left_color_folder,'*.png'));

for i=1:size(left_color_files,1)
    
    name = left_color_files(i).name(1:end-4);
    
    %Get disparity image path
    disparity_path = fullfile(disparity_filestorage_folder,[name '.yml']);
    
    %Get color image path
    left_color_path = fullfile(left_color_folder,[name '.png']);
    
    %Get filtered pcd file path
    pcd_filtered_path = fullfile(pcd_filtered_folder,[name '.pcd']);
    
    %Get detections file path
    detections_path = fullfile(detections_folder,name);
    
    %Load disparity image
%     S=cv.FileStorage(disparity_path);
%     disparity = S.disparity_image;
    
    %Load left color image
    left_color = im2double(imread(left_color_path));
    
    %Load filtered pcd file
%     pcd = loadpcd(pcd_filtered_path);
    
    %Load detections
    load(detections_path);
    
    if size(bbox,1)>0

        component = bbox(1,5);

        for k=1 %:size(bbox,1)
%         showboxes(left_color, [bbox(k,1:4) fbbox(k,:)]);
        image(left_color)
        set(gcf,'position',[50 350 1800 600])
        set(gca, 'LooseInset', [0 0 0 0] )
        
        hold on
        centers = bboxcenterpos([bbox(k,1:4) fbbox(k,:)]);
        centers(1,:)=[];
%         centers(end,:)=[]; %There is one extra box
        
        text(centers(1,1),bbox(1,2)-20,num2str(component),'color',[1 0 0],'fontsize',20,'FontWeight','bold');
        plot(centers(1,1),centers(1,2),'ro','MarkerFaceColor',[1 0 0],'MarkerSize',12);
        
        for l=2:size(centers,1)
            id=l-1;
            
            plot(centers(l,1),centers(l,2),'bo','MarkerFaceColor',[0 0 1],'MarkerSize',12);
            text(centers(l,1),centers(l,2),num2str(id),'color',[1 1 0],'HorizontalAlignment','center');
        end
     
        pause(1)
        end
    end
    
end