import numpy as np
import os
import rospy
from math import acos, degrees

def calculate_distance(point1, point2):
    return np.linalg.norm(np.array(point1) - np.array(point2))

def calculate_angle_with_xy_plane(point1,point2):
    # 计算方向向量
    dir_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], point2[2] - point1[2]])
    
    # 计算方向向量在 xy 平面上的投影
    proj_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], 0])
    
    # 计算夹角
    angle = acos(np.dot(dir_vector, proj_vector) / (np.linalg.norm(dir_vector) * np.linalg.norm(proj_vector)))
    angle_deg = degrees(angle)

    return angle_deg

def read_points_from_file(file_path):
    """从文件中读取焊缝数据"""
    with open(file_path, 'r') as file:
        lines = file.readlines()
    data = [line.strip().split('/') for line in lines]
    points_list = [[tuple(map(float, pair.split(','))) for pair in line] for line in data]
    return points_list
########
# 从文件中读取焊缝起点和终点，并计算两点构成的向量
def get_hanfeng(file_path):

    data_ping, data_shu = [],[]

    with open(file_path, 'r') as file:
        # 逐行读取文件内容
        lines = file.readlines()
    flag_sequence = 0
    for line in lines:
        flag_sequence += 1
        # 去除行尾的换行符并按'/'分割每一行
        points_str = line.strip().split('/')
        
        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]

        angle = calculate_angle_with_xy_plane(point1,point2)
        
        if abs(angle) < 30:     #与xy平面夹角小于30度
            data_ping.append([point1, point2, flag_sequence])
        else:
            data_shu.append([*sorted([point1, point2], key=lambda p: p[2]), flag_sequence]) 

        data = data_ping + data_shu

        # midpoint = [(p2 + p1)/2 for p1, p2 in zip(point1, point2)]
        # vector = [p2 - p1 for p1, p2 in zip(point1, point2)]
        
        # start_points.append(point1)
        # end_points.append(point2)
        # midpoints.append(midpoint)  
        # hanfeng.append(vector)   
    # return start_points, end_points, midpoints, hanfeng
    return data_ping, data_shu, data


################################################################################################################
################################################################################################################
def sort_welds_by_distance_ping(data_ping, reference_point):
    """根据与参考点的距离对焊缝进行排序"""
    sorted_data_ping = []
    for i in range(len(data_ping)):
        # 计算起点到参考点的距离并排序
        data_with_distances = [[calculate_distance(start, reference_point),calculate_distance(end, reference_point), start, end,flag_sequence] for start, end,flag_sequence in data_ping]
        for i in range(len(data_with_distances)):
            if data_with_distances[i][0] > data_with_distances[i][1]:
                data_with_distances[i][0] = data_with_distances[i][1]
                tem = data_with_distances[i][2]
                data_with_distances[i][2] = data_with_distances[i][3]
                data_with_distances[i][3] = tem
        data_with_distances.sort(key=lambda x: x[0])

        # 初始化结果和当前终点
        sorted_data_ping.append(data_with_distances.pop(0)[-3:])   
        reference_point = sorted_data_ping[-1][1]                   
        data_ping = [[start, end,flag_sequence] for distances1,distances2, start,end,flag_sequence in data_with_distances]
    return sorted_data_ping
def sort_welds_by_distance_shu(data_shu, reference_point):
    """根据与参考点的距离对焊缝进行排序"""
    sorted_data_shu = []
    for i in range(len(data_shu)):
        # 计算起点到参考点的距离并排序
        data_with_distances = [[calculate_distance(start, reference_point), start, end ,flag_sequence] for start, end ,flag_sequence in data_shu]
        
        data_with_distances.sort(key=lambda x: x[0])

        # 初始化结果和当前终点
        sorted_data_shu.append(data_with_distances.pop(0)[-3:])
        reference_point = sorted_data_shu[-1][1]
        data_shu = [[start, end ,flag_sequence] for distances, start,end ,flag_sequence in data_with_distances]
    return sorted_data_shu
def run():
    
    # 定义参考点
    reference_point = (1172.85077147, -1.50259925, 668.30298144)

    # file_path = '/home/chen/catkin_ws/src/publish_pointcloud/data/pointclouddata/20240520r'
    file_path = rospy.get_param("folder_path")
    file_path_pointcloud = os.path.join(file_path, 'pointcloud.txt')
    file_path_points = os.path.join(file_path, 'points.txt')
    file_path_result = os.path.join(file_path, 'result.txt')
    file_path_points_guihua = os.path.join(file_path, 'points_guihua.txt')

    data_ping, data_shu, data = get_hanfeng(file_path_points)

    
    # 对焊缝排序
    sorted_data_ping = sort_welds_by_distance_ping(data_ping, reference_point)
    if len(data_ping) > 0:
        a = tuple(sorted_data_ping[-1][1])
    else:
        a = reference_point
    sorted_welds_shu = sort_welds_by_distance_shu(data_shu, a)
    sorted_welds = sorted_data_ping + sorted_welds_shu

    flag_sequence = [flag_sequence for start,end ,flag_sequence in sorted_welds]
    #传递焊接顺序
    rospy.set_param('welding_sequence',flag_sequence)
    sorted_welds_withnoflag = [[start,end]for start,end ,flag_sequence in sorted_welds]

    with open(file_path_points_guihua, "w") as file:
        for start, end in sorted_welds_withnoflag:
            # 将每一对起点和终点转换为字符串，并用逗号连接，最后以换行符分隔不同的焊缝对
            line = "{},{},{}".format(start[0], start[1], start[2]) + "/" + "{},{},{}".format(end[0], end[1], end[2]) + "\n"
            file.write(line)
    
    rospy.loginfo("Welding sequence calculation completed")

if __name__ == '__main__':    
    run()