clc;
clear all;

out_filename = "test.csv";
header = "pointcloud, numOFpoints1, numOFpoints2, normedS1, normedS2, deg12, spicatost1, spicatost2, sikmost1, sikmost2, dist";
fid = fopen(out_filename,'wt');
fprintf(fid, '%s\n', header);
fclose(fid);

numOFpc = 7

for i = 1:numOFpc
  
  pointcloud = i

  filename1 = sprintf("%02d_cut1.xyz", i)
  filename2 = sprintf("%02d_cut2.xyz", i)

  cloudCUT1 = dlmread(filename1,' '); # Naèti xyz file.
  cloudCUT1 = cloudCUT1(:,1:3);
  
  cloudCUT2 = dlmread(filename2,' '); # Naèti xyz file.
  cloudCUT2 = cloudCUT2(:,1:3);
 
  numOFpoints1 = size(cloudCUT1,1)
  numOFpoints2 = size(cloudCUT2,1)

  cloudCUT1c = cloudCUT1-mean(cloudCUT1);
  cloudCUT2c = cloudCUT2-mean(cloudCUT2);
  
  [U1, S1, V1] = svd(cloudCUT1c, "econ");  # SVD pro oøíznutý point cloud. 
  normedS1 = sqrt(diag(S1).^2/(size(cloudCUT1,1)-1))(3) # Normalizace odchylky rovinatosti.
  [U2, S2, V2] = svd(cloudCUT2c, "econ");
  normedS2 = sqrt(diag(S2).^2/(size(cloudCUT2,1)-1))(3)

  cloudCUT1c3sig = cloudCUT1c(cloudCUT1c(:,3) < 3*normedS1, :);
  cloudCUT2c3sig = cloudCUT2c(cloudCUT2c(:,3) < 3*normedS2, :);
  
  if ((numOFpoints1-size(cloudCUT1c3sig,1))/numOFpoints1 < 0.01)
    
    cloudCUT1c = cloudCUT1c3sig;
    [U1, S1, V1] = svd((cloudCUT1c), "econ");  # SVD pro oríznutý point cloud. 
    normedS1 = sqrt(diag(S1).^2/(size(cloudCUT1c,1)-1))(3); # Normalizace odchylky rovinatosti.
    
  endif

  if ((numOFpoints2-size(cloudCUT2c3sig,1))/numOFpoints2 < 0.01)
    
    cloudCUT2c = cloudCUT2c3sig;
    [U2, S2, V2] = svd((cloudCUT2c), "econ");  # SVD pro oríznutý point cloud. 
    normedS2 = sqrt(diag(S2).^2/(size(cloudCUT2c,1)-1))(3); # Normalizace odchylky rovinatosti.
    
  endif
  
  numOFpoints1 = size(cloudCUT1c,1)
  numOFpoints2 = size(cloudCUT2c,1)

  normal1 = V1(:,3);
  normal2 = V2(:,3);

  dotProduct12 = dot(normal1, normal2);
  deg12 = rad2deg(acos(dotProduct12))
    
  odchylkyZ1 = (U1*S1)(:, 3);
  odchylkyZ2 = (U2*S2)(:, 3);

  odchylkyZ1 = odchylkyZ1(odchylkyZ1(:,1) < 3*normedS1, :);
  odchylkyZ2 = odchylkyZ2(odchylkyZ2(:,1) < 3*normedS2, :);

  sikmost1 = skewness (odchylkyZ1, 0)
  spicatost1 = kurtosis(odchylkyZ1, 0)

  sikmost2 = skewness (odchylkyZ2, 0)
  spicatost2 = kurtosis(odchylkyZ2, 0)

  centr1 = mean(cloudCUT1)(1:3);
  centr2 = mean(cloudCUT2)(1:3);

  p1 = dot(normal1, centr1.')*normal1;
  p2 = dot(normal1, centr2.')*normal1;

  dist = sqrt((p1(1)-p2(1)).^2+(p1(2)-p2(2)).^2+(p1(3)-p2(3)).^2)

  line = [pointcloud, numOFpoints1, numOFpoints2, normedS1, normedS2, deg12, spicatost1, spicatost2, sikmost1, sikmost2, dist];
  dlmwrite(out_filename, line, "-append")

endfor