%% The code is written by Behnaz Pirzamanbein, bepi@dtu.dk last version 2020.08.04
%% detecting layers 
% create result folder
save_folder = 'Results\layer\';
if ~exist(save_folder,'dir')
    mkdir(save_folder)
end 

%% Contrast enhansed volume
switch answer
    case 'No'
        Vout = Vol;
    case 'Yes'
        ce = points.contrast_enhansed_range;
        Vout = (Vol-ce(1))/(ce(2)-ce(1))*ce(2); 
        Vout(Vout < ce(1)) = ce(1); 
        Vout(Vout > ce(2)) = ce(2);
end
%% default

p_sz = size(points.all_slice);
no_slice = points.slice_no(end)-points.slice_no(1)+1;

S = struct('layer',[]);
layers = [];    
surface_XYZ = zeros(p_sz(3),no_slice,3,2);% # of points, # of slices, xyz coordinates, # of layers

h = waitbar(0,'1','Name','Detecting layer ...');
for s = 1:no_slice
    
    % find normals of poitns
    slice_no = points.slice_no(1)+s-1;
    waitbar(s/no_slice,h,sprintf('slice: %d',slice_no))
    
    p_XY = permute(points.all_slice(s,1:2,:),[3,2,1]); % row x 2
    normals = compute_normals(p_XY,1);
    
    % grid along normals
    p_X = p_XY(:,1) + normals(:,1)*range;
    p_Y = p_XY(:,2) + normals(:,2)*range;

    % convert the bend voloume to a stretched volume
    interp_method = 'nearest';
    Vol_conv = interp2(Vout(:,:,slice_no),p_X,p_Y,interp_method);
    
    % find the cost function
    Vol_conv_rotate = permute(Vol_conv,[2,1,3]);
    J = medfilt2(Vol_conv_rotate,[5 5]);
    cost_S = smoothsurface_cost(J,2,20);
    
    % find the layers
    cost_corrected = permute(cost_S,[2 3 1]);
    cost_2 = cat(4,cost_corrected,1-cost_corrected);
    S(s).layer = grid_cut(cost_2,[],[1 1],[],Delta_LU);
       
    layers(:,s,:) = S(s).layer;
    
    % convert back the layers
    for k = 1:size(layers,1)
        % surface 1
        surface_XYZ(k,s,1,1) = p_X(k,layers(k,s,1));
        surface_XYZ(k,s,2,1) = p_Y(k,layers(k,s,1));

        % surface 2
        surface_XYZ(k,s,1,2) = p_X(k,layers(k,s,2));
        surface_XYZ(k,s,2,2) = p_Y(k,layers(k,s,2));
    end
    
    if flag_fig
        figure(1)
        subplot(211)
        imagesc(J),colormap gray
        hold on 
        plot(S(s).layer(:,:,1),'-r')
        plot(S(s).layer(:,:,2),'-r')
        subplot(212)
        imagesc(cost_S)
        hold on 
        plot(S(s).layer(:,:,1),'-r')
        plot(S(s).layer(:,:,2),'-r')
        drawnow
    end
    if flag_fig
        figure(2);
        imagesc(Vout(:,:,slice_no)),colormap gray
        title(['slice ',num2str(s)])
        hold on 
        plot(surface_XYZ(:,s,1,1),surface_XYZ(:,s,2,1),'-c','LineWidth',1)
        plot(surface_XYZ(:,s,1,2),surface_XYZ(:,s,2,2),'-m','LineWidth',1)
        axis image, axis off
        drawnow
    end
end
close(h)

% converted layers
[surface_XYZ(:,:,3,1),~] = meshgrid(1:no_slice,1:p_sz(3));
[surface_XYZ(:,:,3,2),~] = meshgrid(1:no_slice,1:p_sz(3));

%%
if flag_save
    figure
    surf_terrain(layers)
    saveas(gcf,[save_folder,flag_layer,'_',sample,'_',dg,'.png'])
    save([save_folder,flag_layer,'_layers_',sample,'_',dg],'layers')
    
    figure
    surf(surface_XYZ(:,:,1,1),surface_XYZ(:,:,2,1),surface_XYZ(:,:,3,1),...
        'EdgeColor','none','FaceColor','c','FaceAlpha',0.6, 'FaceLighting','phong')
    hold on 
    surf(surface_XYZ(:,:,1,2),surface_XYZ(:,:,2,2),surface_XYZ(:,:,3,2),...
        'EdgeColor','none','FaceColor','m','FaceAlpha',0.6, 'FaceLighting','phong')
    hold off
    saveas(gcf,[save_folder,flag_layer,'_layer_',sample,'_',dg,'.png'])
    save([save_folder,flag_layer,'_converted_layer_',sample,'_',dg],'surface_XYZ')
end

close all

%% if you want to check the results
if flag_fig
    
    B = regularization_matrix_open(p_sz(3),1000,5000);
    for s = 1:points.slice_no(end)-points.slice_no(1)
        fig1 = figure(1);
        slice_no = points.slice_no(1)+s-1;
        imagesc(Vout(:,:,slice_no)),colormap gray
        title(['slice ',num2str(s)])
        hold on 
        s1_XYZ = (squeeze(surface_XYZ(:,s,:,1)))%'*B')';
        s2_XYZ = (squeeze(surface_XYZ(:,s,:,2)))%'*B')';
        plot(s1_XYZ(:,1),s1_XYZ(:,2),'-c','LineWidth',1)
        plot(s2_XYZ(:,1),s2_XYZ(:,2),'-m','LineWidth',1)
        axis image, axis off
        drawnow
        if ~mod(s,20)
            close(fig1)
        end
    end
end