import numpy as np
from scipy.spatial.transform import Rotation as R
import open3d as o3d
from scipy.spatial import KDTree
import tf.transformations as tft
import os
import rospy
from math import acos,tan,atan,degrees,sqrt,sin,cos

def rpy2quaternion(roll, pitch, yaw):#zyx
    x=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2)
    y=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2)
    z=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2)
    w=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2)
    return [x, y, z, w]


def get_hanfeng(file_path):
    """从文件中读取焊缝起点、终点和中间点，并计算两点构成的向量"""
    with open(file_path, 'r') as file:
        # 逐行读取文件内容
        lines = file.readlines()
    start_points, end_points, midpoints , hanfeng = [],[],[],[]
    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]

            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

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 run():
    file_path = rospy.get_param("folder_path")
    file_path_points = os.path.join(file_path, 'points_guihua.txt')
    file_path_result = os.path.join(file_path, 'result.txt')

    result=[]
    start_points, end_points, midpoints, hanfeng = get_hanfeng(file_path_points)
    # 计算位姿
    for i in range(len(hanfeng)): 
        if(calculate_angle_with_xy_plane(start_points[i], end_points[i])<45):#平缝
            q1 = rpy2quaternion(0,0,3*np.pi/4)    #ping
            q2 = rpy2quaternion(0,0,3*np.pi/4)
            result.append(("s", start_points[i], end_points[i], q1, q2))
        else:
            q1 = rpy2quaternion(0,np.pi/4,0)      #shu
            q2 = rpy2quaternion(0,np.pi/4,0)
            result.append(("s", start_points[i], end_points[i], q1, q2))


    # 打开文件准备写入
    with open(file_path_result, "w") as file:
        for group in result:
            # 对于每个数据组，将每个部分用逗号连接，然后在各部分间插入斜杠
            line = "/".join([",".join(map(str, sublist)) for sublist in group]) + "\n"
            file.write(line)

    rospy.loginfo("Welding Angle calculation completed")
if __name__ == '__main__':     
    
    run()