clear all
clc
close all
format long g

source_dir = uigetdir(pwd,'Please select your data directory');

% Get a list of the point clouds
list_PC = dir(fullfile(source_dir,'*M-*.ply')); %All point clouds must be .ply!!
l_PC = length(list_PC); % What is the length?
list_PC(1).name; % Print the name of the first point cloud

pattern = ["BA","VO"];
k = string({list_PC.name}); % Convert list of filenames to string array
B = unique(extractBefore(k, 13)); % find unique file names

% To create a table of results
rng('default') % for reproducibility

% Pre-allocate loop vars
V0 = nan(length(B),1);
V1 = nan(length(B),1);
V2 = nan(length(B),1);
V3 = nan(length(B),1);
V4 = nan(length(B),1);
V5 = nan(length(B),1);
V6 = nan(length(B),1);
V7 = nan(length(B),1);
V8 = nan(length(B),1);

for i=1:length(B)
    k_ = k(find(contains(k,B(i))));
    if sum(contains(k_,pattern(2))) == 0          % In case we do not have BA
        k_vo = k_;
        
        %File name of Veg On point cloud
        pcVO_name = k_vo(1);
        pcVO_fname = fullfile(source_dir,pcVO_name); %Full path to the point cloud
        pcVO = pcread(pcVO_fname); % Read in point cloud
        fprintf('Processing file: %s\n',pcVO_name);
        roi = [0.005 0.495 0.005 0.395 pcVO.ZLimits(1) pcVO.ZLimits(2)]
        
    else                                          % In case we have both BA and VO
        k_vo = k_(contains(k_,pattern(2)));
        k_ba = k_(contains(k_,pattern(1)));
        
        %File name of Veg On point cloud
        pcVO_name = k_vo(1);
        pcVO_fname = fullfile(source_dir,pcVO_name); %Full path to the point cloud
        pcVO = pcread(pcVO_fname); % Read in point cloud
        fprintf('Processing file: %s\n',pcVO_name);
        
        %File name of Veg Off point cloud
        pcBA_name = replace(k_vo(1),pattern(2),pattern(1)) %'MC1-2S-25T-27_BA.ply'; % Try to match the start of the string
        pcBA_fname = fullfile(source_dir,pcBA_name); %Full path to the point cloud
        pcBA = pcread(pcBA_fname); % Read in point cloud
        % Region of interest
        roi = [0.005 0.495 0.005 0.395 pcBA.ZLimits(1) pcVO.ZLimits(2)];
    end
    
    % Region of Interest
    % Vegetation on
    roi_id = findPointsInROI(pcVO,roi);
    pcVO_2 = select(pcVO, roi_id); % Clip to same ROI
    
    % Vegetation off
    roi_idBA = findPointsInROI(pcBA,roi);
    pcBA_2 = select(pcBA, roi_idBA); % Clip to same ROI
    
    threshold = 0.1; % Half a std from the mean of 4 points
    nearNeighbors = 100; % Currently takes a while to run but returns really clean point clouds
    [pcVO_Out, inI, outI] = pcdenoise(pcVO_2,'NumNeighbors',nearNeighbors,'Threshold',threshold);% Veg on
    [pcBA_Out, inBA, outBA] = pcdenoise(pcBA_2,'NumNeighbors',nearNeighbors); % Veg off
    % Veg on
    Zshift = pcVO_Out.ZLimits(1);
    D = zeros(pcVO_Out.Count,3);
    D(:,3) = -Zshift;
    pcD = pctransform(pcVO_Out,D);
    
    %Veg off
    Zshift_BA = pcBA_Out.ZLimits(1);
    D_BA = zeros(pcBA_Out.Count,3);
    D_BA(:,3) = -Zshift;
    pcD_BA = pctransform(pcBA_Out,D_BA);
    
    [groundPtsIdx,nonGroundPtCloud,groundPtCloud] = segmentGroundSMRF(pcD,'ElevationThreshold',0.11, ...
        'SlopeThreshold',0.1, "ElevationScale",0.9,"MaxWindowRadius",10);
    
    tic % Starts stopwatch timer
    
    [groundptCloud_GM1,vegptCloud_GM1,idx1]=Curve_GM(groundPtCloud,50);
    [groundptCloud_GM2,vegptCloud_GM2,idx2]=Curve_GM(groundptCloud_GM1,200);
    
    toc % elapsed time
    
    ptCloudOut = pcmerge(vegptCloud_GM1, vegptCloud_GM2,0.000001);
    ptCloudOut = pcmerge(nonGroundPtCloud,ptCloudOut,0.000001);
    
    [groundptCloud1,vegptCloud1,idx3] = GMM(groundptCloud_GM2,2);
    [groundptCloud2,vegptCloud2,idx4] = GMM(vegptCloud1,2);
    
    ptCloudOut_G = pcmerge(groundptCloud1, groundptCloud2,0.000001);
    
    ptCloudOut_V = pcmerge(vegptCloud2,ptCloudOut,0.000001);
    
    [groundptCloud_BA,vegptCloud_BA,idx_BA] = GMM(pcD_BA,2);
    
    % Grid size for fitting a surface to point clouds
    groundStep = 0.01; % Define for the ground raster
    
    % Fit a surface to the ground class
    [xq_g,yq_g,Output_smooth] = min_surface(ptCloudOut_G,groundStep,0.5,0.4);
    
    %%
    % Grid size for fitting a surface to point clouds
    
    % % Fit a surface to the bare soil point clouds
    [xq_BA,yq_BA,Output_smooth_BA] = min_surface(groundptCloud_BA,groundStep,0.5,0.4);
    %
    volume_litter = sum(sum(abs(Output_smooth - Output_smooth_BA)*100*(groundStep*100)^2))     % in cm^3
    
    pixel_size = 0.005;                                 % pixel size in meters (default = 0.005)
    volume_veg = voxel_volume(ptCloudOut_V,pixel_size)    % Volume in cm^3
    
    volume_Soil = voxel_volume(ptCloudOut_G,pixel_size)    % Volume in cm^3
    
    volume_plot = voxel_volume(pcD,pixel_size)          % Volume in cm^3
    
    volume_BA = voxel_volume(groundptCloud_BA,pixel_size)          % Volume in cm^3
    
    volume_nogaps = volume_litter + volume_veg
    
    [TriIdx, vol] = boundary(double(ptCloudOut_V.Location(:,1)),...
        double(ptCloudOut_V.Location(:,2)),double(ptCloudOut_V.Location(:,3)),1);
    volume_convex_veg = vol*100^3
    
    [TriIdx, volume_convex_plot] = convhull(double(pcD.Location(:,1)),double(pcD.Location(:,2)),double(pcD.Location(:,3)));
    volume_convex_plot*100^3
    
    PC_abv = pc_abv_surface(xq_g,yq_g,Output_smooth,pcD);
    % voxel volume
    volume_voxel_veg_abv_surf = voxel_volume(PC_abv,pixel_size)    % Volume in cm^3
    
    % conex hull
    [TriIdx_abv_surf, vol_abv_surf] = boundary(double(PC_abv.Location(:,1)),...
        double(PC_abv.Location(:,2)),double(PC_abv.Location(:,3)),1);
    volume_convex_veg_abv_surf = vol_abv_surf*100^3    
    
    if length(k_) == 2
        
        pcBA = pcread(pcBA_fname); % Read in point cloud
        
        % Vegetation off
        roi_idBA = findPointsInROI(pcBA,roi);
        pcBA_2 = select(pcBA, roi_idBA); % Clip to same ROI
        
        
        [pcBA_Out, inBA, outBA] = pcdenoise(pcBA_2,'NumNeighbors',nearNeighbors); % Veg off
        
        %Veg off
        Zshift_BA = pcBA_Out.ZLimits(1);
        D_BA = zeros(pcBA_Out.Count,3);
        D_BA(:,3) = -Zshift;
        pcD_BA = pctransform(pcBA_Out,D_BA);
        
        [groundptCloud_BA,vegptCloud_BA,idx_BA] = GMM(pcD_BA,2);
        
        
        % Fit a surface to the bare soil point clouds
        [xq_BA,yq_BA,Output_smooth_BA] = min_surface(groundptCloud_BA,groundStep,0.50,0.40);
        
        PC_abv_BA = pc_abv_surface(xq_BA,yq_BA,Output_smooth_BA,pcD);
        % voxel volume
        volume_voxel_veg_abv_BA_surf = voxel_volume(PC_abv_BA,pixel_size)    % Volume in cm^3
        
        % conex hull
        [TriIdx_abv_BA_surf, vol_abv_BA_surf] = boundary(double(PC_abv_BA.Location(:,1)),...
            double(PC_abv_BA.Location(:,2)),double(PC_abv_BA.Location(:,3)),1);
        volume_convex_veg_abv_BA_surf = vol_abv_BA_surf*100^3
        
        volume_litter = sum(sum(abs(Output_smooth - Output_smooth_BA)*100*(groundStep*100)^2))     % in cm^3
        if any(Output_smooth - Output_smooth_BA < 0)
            print('Point cloud below the bare soil')
        end
        
        
        volume_nogaps = volume_litter + volume_veg
        volume_plot/volume_nogaps*100
    end
    
    V1(i) = volume_plot;
    V2(i) = volume_veg;
    V3(i) = volume_convex_veg;
    if length(k_) == 2
        V4(i) = volume_litter;
    end
    V5(i) = volume_voxel_veg_abv_surf;
    V6(i) = volume_convex_veg_abv_surf;
    V7(i) = volume_convex_veg_abv_BA_surf;
    V8(i) = volume_voxel_veg_abv_BA_surf;
    
    %     clear pcBA pcBA_2 pcBA_Out
    %     clearvars -except B V1 V2 V3 V4 V5 V6 k list_PC pattern source_dir l_PC
end
V0 = array2table(B.','VariableNames',{'Point clouds'});
% % Build table
T = table(V0,V1,V2,V3,V4,V5,V6,V7,V8,'VariableNames',{'Point clouds','Plot volume',...
    'Vegetation volume','Convex hull volume', 'Volume between two surfaces',...
    'Voxel volume veg_abv_surf','Convex volume veg_abv_surf',...
    'Convex volume veg_abv_BA_surf','Voxel volume veg_abv_BA_surf'});
% writetable(splitvars(T),'Ptcs_Volume2021_02072022_2vxl_size.txt','Delimiter',' ')
writetable(splitvars(T),'Ptcs_Volume2021_12082022_5vxl_size.txt','Delimiter',' ')
