import socket
import json
import numpy as np

# import scipy.io

## # # # # # #  总配置参数
dt = 0.005 # 总体时间间隔

## # # # # # # 总体时间匀速，挪动过去
Time_Total_1=2

# 二、将起始位姿读取
try:
    # 打开 data.txt 文件以读取数据
    with open('data.txt', 'r') as file:
        # 解析文件中的 JSON 数据为 Python 字典
        data = json.load(file)

    # 从字典中提取 angle1 - angle5 数据
    Angle1 = data["angle1"]
    Angle2 = data["angle2"]
    Angle3 = data["angle3"]
    Angle4 = data["angle4"]
    Angle5 = data["angle5"]

    # 从字典中提取 PS1 - PS5 数据，并转换为 NumPy 数组
    PS1 = np.array(data["PS1"], dtype=np.float64)
    PS2 = np.array(data["PS2"], dtype=np.float64)
    PS3 = np.array(data["PS3"], dtype=np.float64)
    PS4 = np.array(data["PS4"], dtype=np.float64)
    PS5 = np.array(data["PS5"], dtype=np.float64)

    # 打印读取到的数据（可根据需要选择是否保留）
    print("Angle1:", Angle1)
    print("Angle2:", Angle2)
    print("Angle3:", Angle3)
    print("Angle4:", Angle4)
    print("Angle5:", Angle5)
    print("PS1:", PS1)
    print("PS2:", PS2)
    print("PS3:", PS3)
    print("PS4:", PS4)
    print("PS5:", PS5)

except FileNotFoundError:
    print("未找到 data.txt 文件，请检查文件路径和文件名是否正确。")
except json.JSONDecodeError:
    print("data.txt 文件不是有效的 JSON 格式，请检查文件内容。")
except KeyError as e:
    print(f"数据中缺少所需的键 {e}，请检查 data.txt 文件内容。")
except Exception as e:
    print(f"读取文件时出现其他错误: {e}")

def check_joint_continuity(NNNNN):
    # 将输入转换为 NumPy 数组
    joint_array = np.array(NNNNN)
    rows, Lie = joint_array.shape
    # 若行数小于 2，无法进行连续性检查
    if rows < 2:
        print("由于数据行数少于 2，无法进行连续性检查。")
        return
    # 计算相邻两行的差值的绝对值
    differences = np.abs(np.diff(joint_array, axis=0))
    # 找出这些差值中的最大值
    max_difference = np.max(differences)
    # 根据最大值判断轨迹是否连续
    if max_difference > 1:
        print("轨迹可能不连续")
    else:
        print("关节连续")

# 定义一个函数来计算路径点
def calculate_path(start_pose, end_pose, time_total, dt):
    # 提取起始点和终点的位置信息
    start_position = np.array(start_pose[:3], dtype=np.float64)
    end_position = np.array(end_pose[:3], dtype=np.float64)
    # 提取起始点的姿态信息
    orientation = start_pose[3:]
    # 计算总步数
    total_steps = int(time_total / dt)
    # 初始化路径点列表
    path_points = []
    # 计算每一步的位移增量
    step_vector = (end_position - start_position) / total_steps
    # 计算路径上的每个点
    for step in range(total_steps + 1):
        current_position = start_position + step * step_vector
        # 将位置和姿态组合成一个完整的点
        current_point = np.concatenate((current_position, orientation))
        path_points.append(current_point)
    return path_points

# 计算从 PS3 到 PS4 的路径点
path_3_to_4 = calculate_path(PS3, PS4, Time_Total_1, dt)
print("从 PS3 到 PS4 的路径点：")
for i, point in enumerate(path_3_to_4):
    print(f"第 {i} 步的点: {point}")
# 计算从 PS5 到 PS1 的路径点
path_5_to_1 = calculate_path(PS5, PS1, Time_Total_1, dt)
print("\n从 PS5 到 PS1 的路径点：")
for i, point in enumerate(path_5_to_1):
    print(f"第 {i} 步的点: {point}")