import time
import numpy as np

# 导入运动学函数
from calculate.ik import inverseF

# 导入串口控制函数
from duoji_test.motor2 import send_hex_command, generate_position_command

# 机械臂参数（和 qt_main.py 保持一致）
L1 = 250
L2 = 300
L3 = 300
L4 = 250
L0 = 250

# 串口配置
MOTOR1_PORT = "/dev/ttyACM1"  # 电机1使用的串口
MOTOR2_PORT = "/dev/ttyACM0" # 电机2使用的串口
BAUD_RATE = 115200

# 控制参数
SPEED_MOTOR1 = 10  # Rad/s
SPEED_MOTOR2 = 10  # Rad/s
DIRECTION_MOTOR1 = 1  # 0: 逆时针, 1: 顺时针
DIRECTION_MOTOR2 = 1

# 初始化电机角度为0
current_angle_motor1 = 0
current_angle_motor2 = 0


def control_two_motors(theta1, theta4):
    """
    根据 theta1 和 theta4 控制两个电机转动指定角度
    :param theta1: 左臂角度（弧度）
    :param theta4: 右臂角度（弧度）
    """
    global current_angle_motor1, current_angle_motor2

    # 弧度转角度
    angle1_target = np.degrees(theta1)
    angle4_target = np.degrees(theta4)

    # 计算相对角度
    relative_angle1 = angle1_target - current_angle_motor1
    relative_angle4 = angle4_target - current_angle_motor2

    print(f"发送相对角度: 电机1={relative_angle1:.2f}°, 电机2={relative_angle4:.2f}°")

    # 更新当前角度
    current_angle_motor1 += relative_angle1
    current_angle_motor2 += relative_angle4

    # 生成指令
    cmd1 = generate_position_command(angle=relative_angle1, speed=SPEED_MOTOR1, direction=DIRECTION_MOTOR1)
    cmd2 = generate_position_command(angle=relative_angle4, speed=SPEED_MOTOR2, direction=DIRECTION_MOTOR2)

    # 发送指令到两个电机
    send_hex_command(MOTOR1_PORT, BAUD_RATE, cmd1)
    send_hex_command(MOTOR2_PORT, BAUD_RATE, cmd2)


def run_trajectory():
    """
    主函数：模拟轨迹点并控制电机
    """

    # 模拟一个圆形轨迹（你可以替换成从文件读取或用户输入）
    center = (150, 250)
    radius = 60
    num_points = 100
    angles = np.linspace(0, 2 * np.pi, num_points)
    x_list = center[0] + radius * np.cos(angles)
    y_list = center[1] + radius * np.sin(angles)

    print("开始执行轨迹控制...")

    for i in range(len(x_list)):
        x = x_list[i]
        y = y_list[i]

        try:
            theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
            control_two_motors(theta1, theta4)
        except Exception as e:
            print(f"第 {i} 点计算失败: {e}")

        time.sleep(0.1)  # 控制发送频率（可根据实际需求调整）

    print("轨迹执行完成。")


if __name__ == "__main__":
    run_trajectory()