%% Reset

clear all
close all
clc

addpath ./src/
addpath ./mex/
addpath ./utils/

%% Choose the parameters

Npatches    	= 100;
overlap_ratio   = 0.5;

%% Load the data

scans  	= cell(8, 1);
K       = cell(8, 1);
T       = cell(8, 1);

[FV0.vertices, FV0.faces] = read_ply('./data/rooster/rooster.ply');

for i = 1: 8
    [scans{i}, K{i}]    = read_range_grid_ply(sprintf('./data/rooster/rooster_scan%d.ply', i));
    T{i}                = read_xf(sprintf('./data/rooster/rooster_scan%d.xf', i));
end

clear i

%% Sample the scans on the mesh

FVC = cell(8, 1);

for i = 1: 8
    FVC{i} = sample_image_mesh(FV0, scans{i}, K{i}, T{i});
end

FV          = FV0;
FV.vertices = FVC{8}.vertices;

clear i

%% Segment the mesh

options.verbose                         = true;
options.seeds                        	= [];
[sampled_keypoints, I_vertex_nkeypoint] = segment_mesh_uni(FV, Npatches, options);

clear options

%% Grow the patches

options.verbose             = true;
[grown_patches, patches]    = grow_patches_proj(FV, sampled_keypoints, I_vertex_nkeypoint(:, Npatches), overlap_ratio, options);
patches2                    = grow_patches_proj(FV, sampled_keypoints, I_vertex_nkeypoint(:, floor(Npatches / 2)), 0, options);

clear options

%% Stitch the scans

options.selec    	= false;
options.blend     	= false;
FVC_vertex_avg    	= stitch_scans_mesh(FV, FVC, [], options);

options.selec   	= true;
options.blend    	= false;
FVC_vertex_ang   	= stitch_scans_mesh(FV, FVC, [], options);

options.selec   	= true;
options.blend     	= false;
FVC_patch_ang    	= stitch_scans_mesh(FV, FVC, grown_patches, options);

options.selec   	= true;
options.blend     	= true;
options.lambda    	= 1e-6;
FVC_patch_ang_h_n	= stitch_scans_mesh(FV, FVC, patches2, options);

options.selec   	= true;
options.blend     	= true;
options.lambda  	= 1e-6;
FVC_patch_ang_f_n	= stitch_scans_mesh(FV, FVC, patches, options);

options.selec   	= true;
options.blend     	= true;
options.lambda  	= 1e-6;
FVC_patch_ang_f_f	= stitch_scans_mesh(FV, FVC, grown_patches, options);

clear options

%% Display the results

figure

for i = 1: 8
    subplot(1, 8, i)
    my_patch(FVC{i}, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
    title(sprintf('Scan %d', i))
    axis equal
    axis off
end

clear i

figure

subplot(1, 6, 1)
patch(FVC_vertex_avg, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
title('Average')
axis equal
axis off

subplot(1, 6, 2)
patch(FVC_vertex_ang, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
title('Vertex angle')
axis equal
axis off

subplot(1, 6, 3)
patch(FVC_patch_ang, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
title('Patch angle')
axis equal
axis off

subplot(1, 6, 4)
patch(FVC_patch_ang_h_n, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
title(sprintf('M = %d, \\sigma = %f', floor(Npatches / 2), 0))
axis equal
axis off

subplot(1, 6, 5)
patch(FVC_patch_ang_f_n, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
title(sprintf('M = %d, \\sigma = %f', Npatches, 0))
axis equal
axis off

subplot(1, 6, 6)
patch(FVC_patch_ang_f_f, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
title(sprintf('M = %d, \\sigma = %f', Npatches, overlap_ratio))
axis equal
axis off

figure

tmpFVC = FVC_patch_ang_f_f;

for i = 1: 8
    subplot(1, 8, i)
    tmpFVC.vertices = FVC{i}.vertices;
    patch(tmpFVC, 'FaceColor', 'interp', 'EdgeColor', 'none', 'FaceLighting', 'none')
    title(sprintf('M = %d, \\sigma = %f', Npatches, overlap_ratio))
    axis equal
    axis off
end

clear i tmpFVC