%% Extract the contour of point cloud data
%
%  This function generates a series of smooth curves and writes the curve data to txt text.The preset parameters cannot handle all the data. Please adjust the parameters
%% Import point cloud data

clear;close all
addpath 'D:\EM_work\MT3D\mesh\Modeling script\lib'

ptCloud = pcread('cave02m.ply');
% pcviewer(ptCloud);
% load SEG_Saltxyz;
% ptCloud = pointCloud(p);

%% Define parameter

% function ptCloud2Curve(ptCloud)
gridStep = 0.1;      % Cloud point downsampling interval
nslice   = 30;       % Number of point cloud slices
planeID  = [1 3 2];  % xy plane:[1 2 3],xz plane:[3 2 1]; yz plane:[1 3 2]
DisScale = 0.5;      % Distance scaling factor was extracted from point cloud slice
bsf      = 0.3;      % boundary shrink factor ,between 0 and 1
sm       = 10 ;      % The  larger sm is, the smoother the output will be
npoint   = 100 ;     % Number of points produced by each slice

%% Cloud point downsampling

% ptCloud = pcdenoise(ptCloud);
ptCloud = pcdownsample(ptCloud,'gridAverage',gridStep);
p       = double(ptCloud.Location) ;
p       = p-mean(p);


figure(100) ;
scatter3(p(:,1),p(:,2),p(:,3),2,p(:,3),"filled");

%% Select the direction of the slice

p        = p(:,planeID);

% Add random noise to point cloud only in theoretical model research. Please comment this line when processing measured data

% p        = p+ p.*0.05.*(rand(size(p))-0.5);

%% Point cloud contour extraction
% slice-Boundary- smooth-interpolation-output

maxp3 = max(p(:,3));
minp3 = min(p(:,3));
z    = linspace(maxp3,minp3,nslice);
dis   = DisScale*(maxp3-minp3)/2/nslice ; %Point cloud slice extraction distance
for i=1:1:nslice
    % Point cloud slice extraction
    Pn = p(p(:,3)<z(i)+dis & p(:,3)>z(i)-dis,:);
    % figure (200)
    % scatter3(pn(:,1),pn( :,2),pn(:,3),1,"filled");

    % Point cloud boundary
    k = boundary(Pn(:,1),Pn(:,2),bsf);
    % figure (200);hold on   
    % plot3(pn(k,1),pn(k,2),z(i)*ones(size(k)),'k--')

    % Boundary curve smooth
    PS = smooth1q(complex(Pn(k,1)',Pn(k,2)'),sm,'robust');
  
    % 
    % figure (200);hold on
    % plot3(real(PS),imag(PS),z(i)*ones(size(k)),'b','linewidth',1);

    % Bboundary curve interpolation, each slice has the same number of boundary points
    PI = interpclosed(real(PS),imag(PS),0:1/npoint:1, 'spline');

    PI(3,:) = z(i)*ones(1,npoint+1);
    f1 =PI(planeID,:);

    figure(100)
    hold on;
    plot3(f1(1,:),f1(2,:),f1(3,:),'k','LineWidth',1);
    
    axis equal;
    xlabel(['X(m)']);
    ylabel(['Y(m)']);
    zlabel(['Z(m)']);


    plotSlice;
    %%  writing curve data txt
    PI = PI(planeID,:);
    writing_curve_close(i,PI(1,:),PI(2,:),PI(3,:),npoint);
    fclose all;
end

%% Check the contour of the curve and cloud point data from the picture
%

% end

%

function writing_curve_close(i,x,y,z,np)
%    writing curve data txt

filenames =['curve_' num2str(i,'%02d') '.txt'];
fid = fopen(filenames,'w');
fprintf(fid,'%s \r\n','% Model:    Tunnel.mph');
fprintf(fid,'%s \r\n','% Version:  COMSOL');
fprintf(fid,'%s \r\n','% Date:     May 2023');
fprintf(fid,'%s \r\n','% Dimension:      3');
fprintf(fid,'%s \r\n',['% Nodes:' num2str(np)]);
fprintf(fid,'%s \r\n',['% Elements:' num2str(np)]);
fprintf(fid,'%s \r\n','% Expressions:     1' );
fprintf(fid,'%s \r\n','% Description:        Contour');
fprintf(fid,'%s \r\n','% Coordinates') ;
for i=1:np
fprintf(fid,'%12f %12f %12f \r\n',x(i),y(i),z(i));
end
fprintf(fid,'%s \r\n','% Elements (lines)') ;
for i=1:np-1
fprintf(fid,'%d %d \r\n',[i;i+1]);
end
fprintf(fid,'%d %d \r\n',[np;1]);

end