# -*- coding: utf-8 -*-
"""
# @FileName      : weave_example
# @Time          : 2025-03-07 10:29:54
# @description   : 摆动示例
# @detail        : 开关摆动，在线修改摆动参数示例。
"""
import setup_path
import platform
# 根据操作系统导入相应的模块
if platform.system() == 'Windows':
    from Release.windows import xCoreSDK_python
elif platform.system() == 'Linux':
    from Release.linux import xCoreSDK_python
else:
    raise ImportError("Unsupported operating system")
from log import print_log, print_separator
import math
import time
import threading

WELDING = xCoreSDK_python.welding
PYWELDING = xCoreSDK_python.welding.PyWelding
PI = math.pi


def wait_robot(robot: xCoreSDK_python.BaseRobot, ec):
    '''等待运动结束 - 通过查询机械臂是否处于运动中的方式'''
    print_separator("wait_robot", length=80)
    running = True
    while (running):
        time.sleep(0.1)
        st = robot.operationState(ec)
        if (st == xCoreSDK_python.OperationState.idle
                or st == xCoreSDK_python.OperationState.unknown):
            running = False
    print("move finished")


def move_block(robot: xCoreSDK_python.BaseRobot, cmds, ec):
    '''移动到指定位置,并等待执行完'''
    print_separator("move_block", length=80)
    id = xCoreSDK_python.PyString()
    robot.moveAppend(cmds, id, ec)
    robot.moveStart(ec)
    time.sleep(1)
    wait_robot(robot, ec)


def setWeaveParameters(welding: PYWELDING, ec):
    '''设置摆动参数'''
    time.sleep(3)
    frequency = 4
    amplitude = 0.01
    welding.setWeaveParameters(frequency, amplitude, ec, [2, 0, 2])
    print_log("setWeaveParameters", ec)
    print(ec["ec"])


def weave_op(robot: xCoreSDK_python.xMateRobot, ec):
    '''摆动运动'''
    print_separator("weave_op", length=110)
    # --------------- 预操作 ----------------
    toolset = xCoreSDK_python.Toolset()
    robot.setToolset(toolset, ec)
    robot.setDefaultSpeed(100, ec)
    robot.setDefaultZone(10, ec)
    robot.stop(ec)
    robot.moveReset(ec)

    # --------------- 运动到起始点 ----------------
    p1 = xCoreSDK_python.JointPosition([0, PI / 6, -PI / 2, 0, -PI / 3, 0])
    absj0 = xCoreSDK_python.MoveAbsJCommand(p1, 1000, 10)

    id = xCoreSDK_python.PyString()
    robot.moveAppend([absj0], id, ec)  # 先MoveAbsJ运动到焊接起始点，非摆动
    print("Command ID:", id.content())
    print_log("moveAppend", ec)
    robot.moveStart(ec)
    print_log("moveStart", ec)
    wait_robot(robot, ec)

    # --------------- 摆动运动 ----------------
    l1 = [0.440, -0.15, 0.413, PI, 0, PI]
    l2 = [0.440, 0.04, 0.413, PI, 0, PI]
    l3 = [0.582, 0.04, 0.413, PI, 0, PI]
    l4 = [0.582, -0.15, 0.413, PI, 0, PI]
    l5 = [0.556, -0.15, 0.413, PI, 0, PI]
    ml1 = xCoreSDK_python.MoveLCommand(l1, 1000, 10)
    ml2 = xCoreSDK_python.MoveLCommand(l2, 1000, 10)
    ml3 = xCoreSDK_python.MoveLCommand(l3, 1000, 10)
    ml4 = xCoreSDK_python.MoveLCommand(l4, 1000, 10)
    ml5 = xCoreSDK_python.MoveLCommand(l5, 1000, 10)

    frequency_initial = 2
    amplitude_initial = 0.001

    welding: PYWELDING = robot.welding()
    welding.setSwingMotion(0.4, 4, ec)  # 设置加速度，加加速度
    welding.setWeave(True, ec, frequency_initial, amplitude_initial,
                     [0, 0, 0])  # 切换到摆动轨迹，并设置初始摆动频率，幅值和两侧停留时间

    set_weave_params = threading.Thread(target=setWeaveParameters,
                                        args=(welding, ec))
    set_weave_params.start()  # 3秒后设置摆动参数

    move_block(robot, [ml1, ml2, ml3], ec)  # 摆动运动

    welding.setWeave(False, ec)  # 停止摆动

    move_block(robot, [ml4, ml5], ec)  # 正常运动


def weave_example_1(robot: xCoreSDK_python.xMateRobot, ec):
    """摆动示例1,开启摆动的同时设置摆动参数"""
    p1 = xCoreSDK_python.JointPosition([0, PI / 6, -PI / 2, 0, -PI / 3, 0])
    absj0 = xCoreSDK_python.MoveAbsJCommand(p1, 1000, 10)
    move_block(robot, [absj0], ec)  # 先MoveAbsJ运动到焊接起始点，非摆动

    # 开启摆动的同时设置摆动参数
    frequency_initial = 2
    amplitude_initial = 0.01
    welding: PYWELDING = robot.welding()
    # 设置加速度，加加速度
    welding.setSwingMotion(0.4, 4, ec)
    # 切换到摆动轨迹，并设置初始摆动频率，幅值和两侧停留时间
    welding.setWeave(True, ec, frequency_initial, amplitude_initial, [0, 0, 0])

    l1 = [0.440, -0.15, 0.413, PI, 0, PI]
    ml1 = xCoreSDK_python.MoveLCommand(l1, 100, 10)
    move_block(robot, [ml1], ec)  # 摆动运动
    pass


def weave_example_2(robot: xCoreSDK_python.xMateRobot, ec):
    """摆动示例2,开启摆动之后再设置摆动参数，不同的摆动类型"""
    p1 = xCoreSDK_python.JointPosition([0, PI / 6, -PI / 2, 0, -PI / 3, 0])
    absj0 = xCoreSDK_python.MoveAbsJCommand(p1, 1000, 10)
    move_block(robot, [absj0], ec)  # 先MoveAbsJ运动到焊接起始点

    # 开启摆动之后再设置摆动参数，不同的摆动类型
    frequency_initial = 1
    amplitude_initial = 0.01
    welding: PYWELDING = robot.welding()
    welding.setSwingMotion(0.4, 4, ec)
    # 设置加速度，加加速度
    welding.setWeave(True, ec, frequency_initial, amplitude_initial, [0, 0, 0])
    # 切换到摆动轨迹，并设置初始摆动频率，幅值和两侧停留时间

    # welding.setWeaveParameters(frequency_initial, amplitude_initial, ec,
    #                            [2, 0, 2])  # 默认摆动形状
    welding.setWeaveParameters(frequency_initial, amplitude_initial, ec,
                               [0, 0, 0], WELDING.WeavingType.SINE)# 正弦摆动
    

    l1 = [0.440, -0.15, 0.413, PI, 0, PI]
    ml1 = xCoreSDK_python.MoveLCommand(l1, 100, 10)
    move_block(robot, [ml1], ec)  # 摆动运动
    pass


def weave_example_3(robot: xCoreSDK_python.xMateRobot, ec):
    """摆动示例3,开启摆动之后再设置摆动参数，不同的摆动形状，使用参数结构体的方式"""
    p1 = xCoreSDK_python.JointPosition([0, PI / 6, -PI / 2, 0, -PI / 3, 0])
    absj0 = xCoreSDK_python.MoveAbsJCommand(p1, 1000, 10)
    move_block(robot, [absj0], ec)  # 先MoveAbsJ运动到焊接起始点

    # 开启摆动之后再设置摆动参数，不同的摆动形状，使用参数结构体的方式
    frequency_initial = 2
    amplitude_initial = 0.01
    welding: PYWELDING = robot.welding()
    welding.setSwingMotion(0.4, 4, ec)
    # 设置加速度，加加速度
    welding.setWeave(True, ec, 0.1, 0.1, [0, 0, 0])
    # 切换到摆动轨迹，并设置初始摆动频率，幅值和两侧停留时间

    wp = WELDING.WeaveParams()
    wp.frequency = 2
    wp.amplitude = 0.01
    wp.weaveing_type = WELDING.WeavingType.CIRCULAR
    wp.dwell_type = WELDING.DwellType.WEAVE_STOP
    wp.dwell_time = [0, 0, 0]
    wp.inclination_angle = 0
    wp.elevation_angle = [0, 0]
    wp.radius = 0.01
    # 圆形摆动时有效
    wp.phase_invert = True
    welding.setWeaveParameters(wp, ec)

    l1 = [0.440, -0.15, 0.413, PI, 0, PI]
    ml1 = xCoreSDK_python.MoveLCommand(l1, 1000, 10)
    move_block(robot, [ml1], ec)  # 摆动运动
    pass


def weave_example_4(robot: xCoreSDK_python.xMateRobot, ec):
    """摆动示例4,开启摆动之后在线修改参数"""
    p1 = xCoreSDK_python.JointPosition([0, PI / 6, -PI / 2, 0, -PI / 3, 0])
    absj0 = xCoreSDK_python.MoveAbsJCommand(p1, 1000, 10)
    move_block(robot, [absj0], ec)  # 先MoveAbsJ运动到焊接起始点

    #开启摆动之后在线修改参数
    welding: PYWELDING = robot.welding()
    welding.setSwingMotion(0.4, 4, ec)
    # 设置加速度，加加速度
    welding.setWeave(True, ec, 0.1, 0.1, [0, 0, 0])
    # 切换到摆动轨迹，并设置初始摆动频率，幅值和两侧停留时间

    wp = WELDING.WeaveParams()
    wp.frequency = 2
    wp.amplitude = 0.01
    wp.weaveing_type = WELDING.WeavingType.CIRCULAR
    wp.dwell_type = WELDING.DwellType.WEAVE_STOP
    wp.radius = 0.01
    # 圆形摆动时有效
    wp.phase_invert = True

    welding.setWeaveParameters(wp, ec)
    l1 = [0.440, -0.15, 0.413, PI, 0, PI]
    ml1 = xCoreSDK_python.MoveLCommand(l1, 100, 10)
    id = xCoreSDK_python.PyString()
    robot.moveAppend(ml1, id, ec)
    robot.moveStart(ec)

    time.sleep(2)
    wp.frequency = 4
    wp.radius = 0.01
    wp.phase_invert = False
    welding.setWeaveParameters(wp, ec)

    wait_robot(robot, ec)


if __name__ == '__main__':
    try:
        # 连接机器人
        # 不同的机器人对应不同的类型
        ip = "192.168.0.160"
        robot = xCoreSDK_python.xMateRobot(ip)
        ec = {}
        weave_op(robot, ec)
    except Exception as e:
        print(f"An error occurred: {e}")
