# 如此设定每个点占据的体积：确保比例为ratio_factor的点周围至少有neighbor_point_min_num个点和自己连通。

import open3d as o3d
import numpy as np
import math
import sys
import random

ratio_factor = 0.9
neighbor_point_min_num = 8

# 读取点云文件
pcd = o3d.io.read_point_cloud("./data/PointClound_01.ply")
print(pcd)
# o3d.visualization.draw([pcd])

# 原始点数量
pcd_size = np.asarray(pcd.points).size / 3
print('原始点数量：', pcd_size)

# # 粗暴降采样
# down_sampling_size = 1
# pcd_down = pcd.voxel_down_sample(voxel_size=down_sampling_size)
# pcd_down_size = np.asarray(pcd_down.points).size / 3

# # 酌情降采样，直到点数量降到阈值以下
# down_sampling_size = 0.01
# pcd_down_size = float('inf')
# while(pcd_down_size > 1000):
#   down_sampling_size = down_sampling_size + 0.02
#   pcd_down = pcd.voxel_down_sample(voxel_size=down_sampling_size)
#   pcd_down_size = np.asarray(pcd_down.points).size / 3
#   if down_sampling_size > 10:
#     break

# print('降采样后点数量：', pcd_down_size)
# print('降采样尺度：', down_sampling_size)
# o3d.visualization.draw([pcd])

# bbox情况
# obb = pcd.get_oriented_bounding_box()
# print('外包盒子体积：', obb.volume())

# # fit to unit cube
# pcd.scale(1 / np.max(pcd.get_max_bound() - pcd.get_min_bound()),
#           center=pcd.get_center())
# o3d.visualization.draw_geometries([pcd])

# 建立kd tree
print('build kdtree...')
pcd_tree = o3d.geometry.KDTreeFlann(pcd)
print('build over')

# 对每个采样点，记录其与周围点的距离
sample_num = 0
if pcd_size <= 10000:
  sample_num = pcd_size
else:
  sample_num = 10000
neighbor_dist_list = np.zeros([sample_num], dtype = float)
for sampleIdx in range(0, math.floor(sample_num)):
  sampleIdxInPcd = random.randrange(0, pcd_size - 1, 1)
  neighbor_num = neighbor_point_min_num + 1 # 真正的最近的那个点是自己
  [k, neighbor_idx_list, _] = pcd_tree.search_knn_vector_3d(pcd.points[sampleIdxInPcd], neighbor_num)
  pointSample = np.asarray(pcd.points)[sampleIdxInPcd]
  for neighborIdx in range(1, neighbor_num):
    pointNeighbor = np.asarray(pcd.points)[neighbor_idx_list[neighborIdx]]
    new_val = math.sqrt(math.pow(pointNeighbor[0] - pointSample[0], 2) + math.pow(pointNeighbor[1] - pointSample[1], 2) + math.pow(pointNeighbor[2] - pointSample[2], 2))
    if new_val > neighbor_dist_list[sampleIdx]:
      neighbor_dist_list[sampleIdx] = new_val
  # print(neighbor_dist_list[sampleIdx])
  # if i < 100:
  #   print('-------------')
  #   print('iterate', i)
  #   print(pointNeighbor, pointSample)
  #   print(pointNeighbor[0] - pointSample[0])
  #   print(math.pow(pointNeighbor[0] - pointSample[0], 2))
  #   print(math.pow(pointNeighbor[0] - pointSample[0], 2) + math.pow(pointNeighbor[1] - pointSample[1], 2) + math.pow(pointNeighbor[2] - pointSample[2], 2))
  #   print(neighbor_dist_list[i])
# exit(0)

# 确定素格尺寸
voxel_size = 0
neighbor_dist_min = np.min(neighbor_dist_list)
neighbor_dist_max = np.max(neighbor_dist_list)
print('距离最大值：', np.max(neighbor_dist_list))
print('距离最小值：', np.min(neighbor_dist_list))
if neighbor_dist_min == neighbor_dist_max:
  voxel_size = neighbor_dist_max
else:
  binNum = 1000
  histogram = np.zeros((binNum), dtype = int) 
  for sampleIdx in range(0, sample_num):
    belongBinIdx = math.floor((neighbor_dist_list[sampleIdx] - neighbor_dist_min) / (neighbor_dist_max - neighbor_dist_min) * binNum)
    if belongBinIdx == binNum:
      belongBinIdx = binNum - 1
    histogram[belongBinIdx] = histogram[belongBinIdx] + 1

  countThreshold = sample_num * ratio_factor
  countNow = 0
  for sampleIdx in range(0, binNum):
    print('-----------')
    print(histogram[sampleIdx], 'bin content value')
    countNow = countNow + histogram[sampleIdx]
    print(countNow, 'current total')
    if countNow >= countThreshold:
      voxel_size = neighbor_dist_min + (neighbor_dist_max - neighbor_dist_min) / binNum * (sampleIdx + 1)
      break
print('voxel_size:', voxel_size)

voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcd, voxel_size=voxel_size)
print('voxel_grid: ', voxel_grid)
total_volume = np.asarray(voxel_grid.get_voxels()).size * (math.pow(voxel_size, 3))
print('点云占据体积：', total_volume)
o3d.visualization.draw_geometries([voxel_grid])

sys.exit(0)