import logging

import numpy as np
import open3d as o3d


def read_stl(stl_path: str, save_path: str, view_pointcloud: bool = False) -> None:
    mesh = o3d.io.read_triangle_mesh(stl_path)
    mesh.scale(0.001, center=[0, 0, 0])
    mesh.compute_vertex_normals()

    pcd_model = mesh.sample_points_uniformly(number_of_points=50000)
    # pcd_model = pcd_model.voxel_down_sample(voxel_size=0.01)

    center = pcd_model.get_center().flatten()
    R = np.eye(4)
    R[0, 3], R[1, 3], R[2, 3] = -center
    pcd_model.transform(R)

    R = np.array(
        [[0, 0, 1, 0],
         [0, 1, 0, 0],
         [-1, 0, 0, 0],
         [0, 0, 0, 1]],
    )

    pcd_model.transform(R)

    if view_pointcloud:
        point = [
            [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
            ]  # 画点：原点、第一主成分、第二主成分
        lines = [
            [0, 1], [0, 2], [0, 3]
        ]  # 画出三点之间两两连线
        colors = [
            [1, 0, 0], [0, 1, 0], [0, 0, 1]
        ]
        # 构造open3d中的LineSet对象，用于主成分显示
        line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(point), lines=o3d.utility.Vector2iVector(lines))
        line_set.colors = o3d.utility.Vector3dVector(colors)

        o3d.visualization.draw_geometries([pcd_model, line_set])

    # success = o3d.io.write_point_cloud(save_path, pcd_model)
    # logging.info(f"PCD_0, 写入结果: {success}")


if __name__ == "__main__":
    read_stl()