#!/usr/bin/env python3
import rospy
import numpy as np
import open3d as o3d
from scipy.spatial.transform import Rotation as R
import os
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField



folder_path = rospy.get_param("folder_path")
file_path = os.path.join(folder_path, 'pointcloud.txt')
file_path_points = os.path.join(folder_path, 'points.txt')
#chen############################################################################################
def cull_pointcloud(data,culling_radius):
    data_retained,data_culled = [],[]
    for i in range(data.shape[0]):  # 遍历每一个点 P

        for j in range(len(vectors)):  # 遍历每一个向量 u
            A = start_points[j] - culling_radius * (vectors[j] / np.linalg.norm(vectors[j]))
            B = end_points[j] + culling_radius * (vectors[j] / np.linalg.norm(vectors[j]))
            aaa = (np.linalg.norm(data[i] - A) + np.linalg.norm(data[i] - B)) <= (2 * culling_radius + np.linalg.norm(B - A))
            distance = np.linalg.norm(np.cross(vectors[j], data[i]-start_points[j])) / np.linalg.norm(vectors[j])

            # 如果有任何一个距离小于等于10，则标记为不满足条件
            if distance <= culling_radius and aaa:
            # if aaa:
                data_culled.append(data[i])
                break
            # 如果该点对所有向量的距离都大于10，则保留该点
            elif j == len(vectors)-1:
                data_retained.append(data[i])

    data_retained = np.array(data_retained)
    data_culled = np.array(data_culled)
    return data_retained,data_culled    

################################点云降采样################################

# 从文件中读取焊缝起点和终点，并计算两点构成的向量
def read_and_calculate_vectors(file_path):
    with open(file_path, 'r') as file:
        # 逐行读取文件内容
        lines = file.readlines()
    start_points,end_points,vectors = [],[],[]
    for line in lines:
        # 去除行尾的换行符并按'/'分割每一行
        points_str = line.strip().split('/')
        
        # 确保每一行都正确地分为两部分
        if len(points_str) == 2:
            point1_str = points_str[0].split(',')
            point2_str = points_str[1].split(',')
            
            # 转换字符串为浮点数列表，构造三维点
            point1 = [float(coord) for coord in point1_str]
            point2 = [float(coord) for coord in point2_str]

            start_points.append(point1)
            end_points.append(point2)
            # 计算向量：向量 = 点2 - 点1
            vector = [p2 - p1 for p1, p2 in zip(point1, point2)]
            
            vectors.append(vector)
    
    return start_points,end_points,vectors

# 读取文件
def load_point_cloud_from_binary_txt(file_path):
    with open(file_path, 'rb') as f:
        binary_data = f.read()

        # 将二进制数据转换为 NumPy 数组
        point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
        # 将数组转换为可写状态
        point_cloud = np.copy(point_cloud)
    return point_cloud




pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(load_point_cloud_from_binary_txt(file_path))
#pcd = pcd.voxel_down_sample(voxel_size = 4)

################################去掉焊缝周围的点云#################################
# 计算焊缝向量
start_points,end_points,vectors = np.array(read_and_calculate_vectors(file_path_points))

data = np.asarray(pcd.points)



# culling_radius2 = float(rospy.get_param('culling_radius'))
# data_retained2,data_culled2 =cull_pointcloud(data,culling_radius2)
# data_scaled2 = np.array(data_retained2) / 1000


data_scaled2 = np.array(data) / 1000





ptCloud_scaled1 = o3d.geometry.PointCloud()

ptCloud_scaled2 = o3d.geometry.PointCloud()
ptCloud_scaled2.points = o3d.utility.Vector3dVector(data_scaled2)

#####################################################################################################################################################################
def build_pointcloud2_msg(points):
    msg = PointCloud2()
    msg.header.stamp = rospy.Time(0)
    msg.header.frame_id = "world"

    if len(points.shape) == 3:
        msg.height = points.shape[1]
        msg.width = points.shape[0]
    else:
        msg.height = 1
        msg.width = len(points)

    msg.fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1)]
    msg.is_bigendian = False
    msg.point_step = 12
    msg.row_step = msg.point_step * points.shape[0]
    msg.is_dense = False
    msg.data = np.asarray(points, np.float32).tobytes()
    return msg

def talker():
    pub1 = rospy.Publisher("/pointcloud/output", PointCloud2, queue_size=10)
    # pub2 = rospy.Publisher("/pointcloud/output2", PointCloud2, queue_size=10)
    rospy.init_node('publish_pointcloud', anonymous=True)
    rate = rospy.Rate(10)

    
    points1 = np.asarray(ptCloud_scaled1.points)
    points2 = np.asarray(ptCloud_scaled2.points)
    # msg1 = build_pointcloud2_msg(points1)
    msg2 = build_pointcloud2_msg(points2)
    
    while not rospy.is_shutdown():

        #机械臂完毕，退出点云发布
        sign_pointcloud = str(rospy.get_param("sign_pointcloud"))
        if sign_pointcloud == "1":
            rospy.set_param("sign_pointcloud",0)
            break

        pub1.publish(msg2)
        # pub2.publish(msg2)
        # print("pointcloud has published,sign_pointcloud ={}".format(sign_pointcloud))
        rate.sleep()

if __name__ == '__main__':     

    talker()