local math             = require "aubo.math"
local M                = {}
local app              = {}
local json             = require("dkjson")

-- 导入焊接算法模块 aubo_math
local aubo_math        = require("aubo_math")

-- 导入多层多道算法模块 lmultipass
local mp               = require("lmultipass")
local realtime         = require 'aubo.realtime'

local poseTrans        = math.poseTrans

local aubo             = require('aubo')
local sched            = sched or aubo.sched
local math             = aubo.math or math
local realtime         = aubo.realtime or realtime

local sleep            = sched.sleep
local thread           = sched.thread
local sync             = sched.sync
local run              = sched.run
local kill             = sched.kill
local halt             = sched.halt

local language_        = "en"

local welding_info_    = "[Welding_Info]"
local welding_error_   = "[Welding_Error]"

--探寻线程
local handler_laser_thread_move_5

local laser_device_config

local enable_weldiv_   = 0
local delayed_updates_ = 0

local laser_points_db_ = {}

local global_offset_   = { false, {}, {} }
--参数定义：{ 是否启用偏移，{模板坐标系}，{工件坐标系}}

-- ====================== 错误码表 ======================
local ErrorCode        = {
    -- 通用错误
    COMMON = {
        SUCCESS = 0,
        TABLE_ERROR = 998,
        UNKNOWN_ERROR = 999
    },
    -- 路点相关错误
    WAYPOINT = {
        POSE_DIMENSION_MISMATCH = 1001,  -- 位姿维度不对
        JOINT_DIMENSION_MISMATCH = 1002, -- 关节角维度对
        EMPTY_WAYPOINT = 1003,           -- 路点为空（无有效路径点）
        DIMENSION_ERR_WAYPOINT = 1004    -- 路点维度错误
    },

    --摆动类型相关错误
    WEAVE = {
        TYPE_ERR = 2001,      --类型错误
        AMPLITUDE_ERR = 2002, -- 振幅错误
        FREQUENCY_ERR = 2003  --频率错误
    },

    --线激光设备错误
    LINELASER = {
        CONNECTFAILED = 3001,    --连接失败
        OPEN_LASER_ERR = 3002,   -- 打开激光失败
        CLOSE_LASER_ERR = 3003,  -- 关闭激光失败
        SET_TEMPLATE_ERR = 3004, -- 设置模板失败
    },


}

local function createDirectReturnTable()
    local t = {}
    setmetatable(t, {
        __index = function(_, key)
            return key -- 找不到键时，直接返回原字符串
        end
    })
    return t
end

local trans_ = {
    ["en"] = createDirectReturnTable(),
    ["zh_CN"] = {
        ["Arc initiation failed"] = "起弧失败",
        ["Pose data is empty or dimension mismatch (expected 6D)"] = "姿态数据为空或数据不匹配（预期为6维)",
        ["Automatic setting of op point failed"] = "自动设置 op 点失败",
        ["Set refframe failed"] = "设置参考坐标系失败",
        ["Multiple channels are empty"] = "多道道数为空",
        ["No weld seam identified"] = "未识别到焊缝",
        ["Inverse solution failed for mp_o_point"] = " o 点逆解计算失败",
        ["Inverse solution failed for mp_p_point"] = " p 点逆解计算失败",
        ["During the welding process, it was detected that the welding machine lost its arc, and the program was stopped."] =
        "焊接过程中检测到焊机断弧，程序停止。",
        ["InverseKinematics failed for laser point"] = "激光点的逆向运动学计算失败",
        ["Run line number"] = "运行行号",
        ["Straight-tooth swing calculation error"] = "直线锯齿摆动计算错误",
        ["Inverse solution failed"] = "逆解失败",
    },
}

local function showErrorMessage(error_message_english)
    local _ENV = sched.select_robot(1)
    clearNamedVariable("_popup_continue_or_stop")
    do
        local _popup_t = getControlSystemTime()

        popup(TraceLevel.ERROR, "Error", trans_[language_][error_message_english], 1)

        halt()
        while not variableUpdated("_popup_continue_or_stop", _popup_t) do sync() end
        if not getBool("_popup_continue_or_stop", true) then halt() end
    end
end

local function syncProgramPoint()
    local _ENV = sched.select_robot(1)
    log_info(welding_info_ .. "Sched.sync_program_point()")
    sched.sync_program_point()
end

-- arc_detection 起弧成功地址
-- arc 起弧地址
-- on_arc_repeat 最大重复次数
-- on_arc_prepare 最长起弧等待时间
-- gas 送气地址
-- given_current 电流地址
-- given_voltage 电压地址
-- 初始化焊接配置

local arc_detection_port = 0
local arc_port = 0
local on_arc_repeat_times = 3
local on_arc_prepare_time = 1
local gas_port = 2
local given_current_port = 0
local given_voltage_port = 1

local function initWeldingConfiguration(arc_detection, arc, on_arc_repeat, on_arc_prepare, gas, given_current,
                                        given_voltage)
    arc_detection_port = arc_detection
    arc_port = arc
    on_arc_repeat_times = on_arc_repeat
    on_arc_prepare_time = on_arc_prepare
    gas_port = gas
    given_current_port = given_current
    given_voltage_port = given_voltage
end

-- 获取电弧IO控制值
local function getArcIoOutput()
    local _ENV = sched.select_robot(1)
    if type(getArcIoValue) == "function" then
        return getArcIoValue()
    else
        return getStandardDigitalInput(arc_detection_port)
    end
end

-- 设置电弧IO输出控制值
local function setArcIoOutput(value)
    local _ENV = sched.select_robot(1)
    if type(setArcIoValue) == "function" then
        setArcIoValue(value)
    else
        setStandardDigitalOutput(arc_port, value)
    end
end

-- 设置气体电磁阀状态（true=打开，false=关闭）
local function setGasSolenoidState(state)
    local _ENV = sched.select_robot(1)
    if type(setGasIoValue) == "function" then
        setGasIoValue(state)
    else
        setStandardDigitalOutput(gas_port, state)
    end
end

-- 设置目标焊接电流输出值
local function setTargetCurrentOutput(current_value)
    local _ENV = sched.select_robot(1)
    if type(setGivenCurrentIoValue) == "function" then
        setGivenCurrentIoValue(current_value)
    else
        setStandardAnalogOutput(given_current_port, current_value)
    end
end

-- 设置目标焊接电压输出值
local function setTargetVoltageOutput(voltage_value)
    local _ENV = sched.select_robot(1)
    if type(setGivenVoltageIoValue) == "function" then
        setGivenVoltageIoValue(voltage_value)
    else
        setStandardAnalogOutput(given_voltage_port, voltage_value)
    end
end


-- current 焊接电流
-- voltage 焊接电压
-- off_voltage 熄弧电压
-- off_current 熄弧电流
-- arc_extinguishing_time 回烧时间
-- delivery_time 延迟送气时间
-- arc_voltage 起弧电压
-- arc_current 起弧电流
-- starting_arc_time 起弧时间
-- 更新焊接参数
local current_ = 0
local voltage_ = 0
local off_voltage_ = 0
local off_current_ = 0
local arc_extinguishing_time_ = 0
local delivery_time_ = 0
local arc_voltage_ = 0
local arc_current_ = 0
local starting_arc_time_ = 0

local detection_arcing_thread = thread(function()
    local _ENV = sched.select_robot(1)
    local count = 0
    local latency_time = 0.2
    while true do
        local successful_arc_initiation = getArcIoOutput()
        if not successful_arc_initiation and count == 5 then
            setArcIoOutput(false)
            log_info(welding_info_ .. "setArcIoOutput:" .. "false")
            sleep(latency_time)
            log_error(welding_error_ .. "Detected welding arc interruption, stop arc starting.")
            showErrorMessage(
                "During the welding process, it was detected that the welding machine lost its arc, and the program was stopped.")
            abort()
        elseif not successful_arc_initiation then
            count = count + 1
            sleep(latency_time)
        else
            if count ~= 0 then
                count = 0
            end
            sleep(latency_time)
        end
    end
end, "Arc_Detection_Thread")

local enable_weldiv_thread = thread(function()
    local _ENV = sched.select_robot(1)
    enable_weldiv_ = 1
    sleep(starting_arc_time_)
    if delayed_updates_ == 0 then
        setTargetCurrentOutput(current_)
        log_info(welding_info_ .. "setTargetCurrentOutput:" .. current_)
        setTargetVoltageOutput(voltage_)
        log_info(welding_info_ .. "setTargetVoltageOutput:" .. voltage_)
    end
    enable_weldiv_ = 0
end, "Enable_WeldIV_Thread")

local delay_update_current_voltage_thread = thread(function()
    local _ENV = sched.select_robot(1)
    delayed_updates_ = 1
    while enable_weldiv_ == 1 do
        sleep(0.1)
    end
    setTargetCurrentOutput(current_)
    log_info(welding_info_ .. "setTargetCurrentOutput:" .. current_)
    setTargetVoltageOutput(voltage_)
    log_info(welding_info_ .. "setTargetVoltageOutput:" .. voltage_)
    delayed_updates_ = 0
end, "Delay_Update_Current_Voltage_Thread")

local function updateWeldingParameters(current, voltage, off_voltage, off_current, arc_extinguishing_time, delivery_time,
                                       arc_voltage, arc_current, starting_arc_time)
    local _ENV = sched.select_robot(1)
    current_ = current
    voltage_ = voltage

    off_voltage_ = off_voltage
    off_current_ = off_current
    arc_extinguishing_time_ = arc_extinguishing_time
    delivery_time_ = delivery_time

    arc_voltage_ = arc_voltage
    arc_current_ = arc_current
    starting_arc_time_ = starting_arc_time

    if (enable_weldiv_ == 1) then
        run(delay_update_current_voltage_thread)
        return
    else
        setTargetCurrentOutput(current_)
        log_info(welding_info_ .. "setTargetCurrentOutput:" .. current_)
        setTargetVoltageOutput(voltage_)
        log_info(welding_info_ .. "setTargetVoltageOutput:" .. voltage_)
    end

    local str = string.format("updateWeldingParameters(%s,%s,%s,%s,%s,%s,%s,%s,%s)", current or 0,
        voltage or 0, off_voltage or 0, off_current or 0,
        arc_extinguishing_time or 0, delivery_time or 0, arc_voltage or 0,
        arc_current or 0, starting_arc_time or 0)

    log_info(welding_info_ .. str)
end

local function updateWeldingParametSIntermittent(welder_param)
    local _ENV = sched.select_robot(1)
    updateWeldingParameters(welder_param.current or 0, welder_param.voltage or 0, welder_param.off_voltage or 0,
        welder_param.off_current or 0, welder_param.arc_extinguishing_time or 0, welder_param.delivery_time or 0,
        welder_param.arc_voltage or 0, welder_param.arc_current or 0, welder_param.starting_arc_time or 0)
end

-- 起弧
local function startArc()
    local _ENV = sched.select_robot(1)
    syncProgramPoint()
    local arcing_successful_0 = false
    local initial_time = 0

    setTargetCurrentOutput(current_)
    log_info(welding_info_ .. "setTargetCurrentOutput:" .. current_)
    setTargetVoltageOutput(voltage_)
    log_info(welding_info_ .. "setTargetVoltageOutput:" .. voltage_)

    while true do
        syncProgramPoint()

        if getArcIoOutput() then
            break
        end

        if (not getArcIoOutput()) then
            local count = 0
            while count < on_arc_repeat_times do
                count = count + 1
                if (arcing_successful_0 == false) then
                    setArcIoOutput(false)
                    log_info(welding_info_ .. "setArcIoOutput:" .. "false")
                    sleep(0.2)
                    setArcIoOutput(true)
                    log_info(welding_info_ .. "setArcIoOutput:" .. "true")
                    initial_time = 0
                    while initial_time < on_arc_prepare_time do
                        if (getArcIoOutput()) then
                            log_info(welding_info_ .. "Start arcing")
                            arcing_successful_0 = true
                            break
                        else
                            initial_time = initial_time + sched.sync_time
                            sync()
                        end
                    end
                    if (arcing_successful_0 == true) then
                        break
                    end
                    setArcIoOutput(false)
                    log_info(welding_info_ .. "setArcIoOutput:" .. "false")
                end
            end
            if (arcing_successful_0 == false) then
                setArcIoOutput(false)
                log_info(welding_info_ .. "setArcIoOutput:" .. "false")
                sleep(0.2)
                log_error(welding_error_ .. "Arc initiation failed")
                showErrorMessage("Arc initiation failed")
                abort()
            end
        end
    end
    run(detection_arcing_thread)
    run(enable_weldiv_thread)
end

-- 熄弧
local function stopArc()
    local _ENV = sched.select_robot(1)
    syncProgramPoint()
    setTargetCurrentOutput(current_)
    log_info(welding_info_ .. "setTargetCurrentOutput:" .. current_)
    setTargetVoltageOutput(voltage_)
    log_info(welding_info_ .. "setTargetVoltageOutput:" .. voltage_)
    sleep(arc_extinguishing_time_)
    kill(detection_arcing_thread)
    setArcIoOutput(false)
    log_info(welding_info_ .. "setArcIoOutput:" .. "false")
    log_info(welding_info_ .. "Start arc blowout")
    setGasSolenoidState(true)
    log_info(welding_info_ .. "setGasSolenoidState:" .. "true")
    sleep(delivery_time_)
    setGasSolenoidState(false)
    log_info(welding_info_ .. "setGasSolenoidState:" .. "false")
    setTargetCurrentOutput(0)
    log_info(welding_info_ .. "setTargetCurrentOutput:" .. 0)
    setTargetVoltageOutput(0)
    log_info(welding_info_ .. "setTargetVoltageOutput:" .. 0)
end

-- ==================== 类型定义 ====================
local MetaInstructionType = {
    ["Nop"] = 0,
    ["Robot_MoveJoint"] = 1,
    ["Robot_MoveJointToReadyPoint"] = 2,
    ["Robot_AirMoveLine"] = 3,
    ["Robot_MoveLineToReadyPoint"] = 4,
    ["Robot_MoveLine"] = 5,
    ["Robot_AirMoveArc"] = 6,
    ["Robot_MoveArc"] = 7,
    ["Robot_WaitArrival"] = 8,
    ["Robot_WeaveStart"] = 9,
    ["Robot_WeaveEnd"] = 10,
    ["Welder_ArcOn"] = 100,
    ["Welder_ArcOff"] = 101,
    ["Welder_ArcOffPra"] = 102,
    ["Welder_UpdateParameter"] = 103,
    ["Robot_MoveWeave"] = 104,
    ["Robot_MoveStitchWeld"] = 105,
    ["MultiLayer_Start"] = 106,
    ["MultiLayer_Stop"] = 107,
    ["LineLaser_Open"] = 200,
    ["LineLaser_Close"] = 201,
    ["Collect_Start"] = 202,
    ["Track_Start"] = 203
}

-- ==================== 工具函数 ====================
local function formatPose(pose)
    if not pose or #pose ~= 6 then
        return "{0,0,0,0,0,0}" -- 返回默认值
    end

    return { pose[1] or 0, pose[2] or 0, pose[3] or 0, pose[4] or 0, pose[5] or 0, pose[6] or 0 }
end

local function formatPoseString(pose)
    -- 添加防御性检查
    if not pose or type(pose) ~= "table" then
        return "nil_pose" -- 或返回默认值 "{0,0,0,0,0,0}"
    end

    -- 确保有6个元素
    local safe_pose = { pose[1] or 0, pose[2] or 0, pose[3] or 0, pose[4] or 0, pose[5] or 0, pose[6] or 0 }

    return string.format("{%f,%f,%f,%f,%f,%f}", safe_pose[1], safe_pose[2], safe_pose[3], safe_pose[4],
        safe_pose[5], safe_pose[6])
end

-- 判断两个点位是否相似（前3位误差<0.0001，后3位误差<0.01）
local function arePointsSimilar(point2, point3)
    -- 如果任一点位为nil，直接返回false
    if not point2 or not point3 then
        return false
    end

    -- 检查点位数据格式是否正确（6个数值）
    if type(point2) ~= "table" or #point2 ~= 6 or type(point3) ~= "table" or #point3 ~= 6 then
        return false
    end

    -- 前3位（位置坐标）误差判断（<0.0001）
    local posThreshold = 0.0001
    local posDiff1 = math.abs(point2[1] - point3[1])
    local posDiff2 = math.abs(point2[2] - point3[2])
    local posDiff3 = math.abs(point2[3] - point3[3])
    if posDiff1 > posThreshold or posDiff2 > posThreshold or posDiff3 > posThreshold then
        return false
    end

    -- 后3位（角度或姿态）误差判断（<0.01）
    local rotThreshold = 0.01
    local rotDiff1 = math.abs(point2[4] - point3[4])
    local rotDiff2 = math.abs(point2[5] - point3[5])
    local rotDiff3 = math.abs(point2[6] - point3[6])
    if rotDiff1 > rotThreshold or rotDiff2 > rotThreshold or rotDiff3 > rotThreshold then
        return false
    end

    -- 所有条件满足，返回true
    return true
end

local function deepCopy(original)
    if type(original) ~= "table" then
        return original -- 如果不是表，直接返回（如 number, string, boolean）
    end

    local copy = {}
    for key, value in pairs(original) do
        copy[key] = deepCopy(value) -- 递归复制每个值
    end
    return copy
end

local function calculateActualSpeed(motion_param)
    local _ENV = sched.select_robot(1)
    local v = motion_param.welding_speed
    local f = motion_param.frequency
    local h = motion_param.amplitude

    if f == 0 then
        log_error(welding_error_ .. "Frequency does not meet the requirements:" .. f)
        return 0.0
    end

    return 4 * f * math.sqrt(
        (v / f * 0.25) ^ 2 + h ^ 2
    )
end

local function inverseSolutionSuccess(point)
    local _ENV = sched.select_robot(1)
    local q, err = inverseKinematics(point.q, point.tcp_pose)

    if err ~= 0 then
        showErrorMessage("Inverse solution failed")
        return false
    end

    return true
end

local function coordinateSystemTransformationOfPoints(pint, base_coordinate_system, target_coordinate_system)
    local _ENV = sched.select_robot(1)
    local template_coordinate_to_base = base_coordinate_system
    local workpiece_coordinate_to_base = target_coordinate_system
    local point_in_base = pint

    --基坐标系转换到工件坐标系
    local base_to_template_coordinate = poseInverse(template_coordinate_to_base)
    local template_to_workpiece = poseTrans(workpiece_coordinate_to_base, base_to_template_coordinate)
    local point_in_Workpiece = poseTrans(template_to_workpiece, point_in_base)
    log_info(welding_info_ .. "point_in_Workpiece" .. formatPoseString(point_in_Workpiece))
    return point_in_Workpiece
end


--计算偏移点
local function calculateOffsetPoint(point)
    if global_offset_[1] then
        log_info(welding_info_ .. "Start the translation")
        log_info(welding_info_ .. "point" .. formatPoseString(point))
        log_info(welding_info_ .. "global_offset_[2]" .. formatPoseString(global_offset_[2]))
        log_info(welding_info_ .. "global_offset_[3]" .. formatPoseString(global_offset_[3]))
        return coordinateSystemTransformationOfPoints(point, global_offset_[2], global_offset_[3])
    end
    return point
end

-- ==================== 验证函数 ====================
local function validatePoint(point)
    if not point then
        return false, "Missing point data"
    end
    local tcp_pose = point.tcp_pose or point.pose -- 兼容两种字段名
    local q = point.q or point.joint              -- 兼容两种字段名
    if not tcp_pose or #tcp_pose ~= 6 then
        return false, ErrorCode.WAYPOINT.POSE_DIMENSION_MISMATCH
    end
    if not q or #q ~= 6 then
        return false, ErrorCode.WAYPOINT.JOINT_DIMENSION_MISMATCH
    end
    return true
end

local function validateWeldingParams(params)
    if not params then
        return true
    end -- 允许空参数
    if params.current and (params.current < 0 or params.current > 500) then
        return false, "Current out of range (50-500A)"
    end
    if params.voltage and (params.voltage < 0 or params.voltage > 40) then
        return false, "Voltage out of range (10-40V)"
    end
    return true
end

-- ==================== 摆动参数生成 ====================
local function getSwingParams(motion_param)
    local _ENV = sched.select_robot(1)
    -- 参数校验与默认值处理
    motion_param = motion_param or {}
    local amplitude = tonumber(motion_param.amplitude) or 0.0
    local frequency = tonumber(motion_param.frequency) or 1.0 -- 默认1Hz防止除零
    local welding_speed = tonumber(motion_param.welding_speed) or 0.0
    local hold_distance = tonumber(motion_param.hold_distance) or 0
    local standing_time = tonumber(motion_param.standing_time)
    local left_standing_time, right_standing_time

    if standing_time and standing_time >= 0.0 then
        left_standing_time = standing_time
        right_standing_time = standing_time
    else
        left_standing_time = motion_param.left_standing_time or 0.0
        right_standing_time = motion_param.right_standing_time or 0.0
    end
    local angle = tonumber(motion_param.angle) or 0
    local direction = tonumber(motion_param.direction) or 0
    local weave = motion_param.weave or 0 -- 默认无摆动

    -- 计算步长（保留8位小数）
    local step = tonumber(string.format("%.8f", welding_speed / frequency))

    -- 摆动类型映射
    local weave_type = ({
        [1] = "SINE",
        [2] = "SPIRAL",
        [3] = "CRESCENT",
        [4] = "ZIGZAG"
    })[weave] or "NONE"

    -- 生成标准JSON格式（不带外层引号）
    local json_str = string.format(
        '{"type":"%s","step":%.8f,"amplitude":[%.8f,%.8f],' ..
        '"hold_distance":[%.8f,%.8f],"hold_time":[%.8f,%.8f],' ..
        '"angle":%.8f,"direction":%d}',
        weave_type,
        step,
        amplitude, amplitude,
        hold_distance, hold_distance,
        left_standing_time, right_standing_time,
        angle,
        direction
    )

    -- 调试输出
    log_info(welding_info_ .. "Generated swing parameters JSON:")
    log_info(welding_info_ .. json_str)
    return json_str
end

-- ==================== 运动指令生成器 ====================
local function generateMoveJoint(point, params)
    local _ENV = sched.select_robot(1)
    local q = point.q or point.joint
    local tcp_pose = point.tcp_pose or point.pose
    tcp_pose = calculateOffsetPoint(tcp_pose)
    local belnd_radius = params.blend_radius or 20.0

    local print_out = string.format("moveJoint(inverseKinematics(%s,%s),%f,%f,%f,0)", formatPoseString(q),
        formatPoseString(tcp_pose), params.motion_parameters.joint_acc or 80.0,
        params.motion_parameters.joint_speed or 60.0, 0.0)
    log_info(welding_info_ .. print_out)

    local inv_q, err = inverseKinematics(formatPose(q), formatPose(tcp_pose))

    if err ~= 0 then
        return
    end

    moveJoint(inv_q, params.motion_parameters.joint_acc or 80.0,
        params.motion_parameters.joint_speed or 60.0, 0.0, 0)

    return print_out
end

local function generateMoveLine(point, speed, params, state, is_welding_endpoints)
    local _ENV = sched.select_robot(1)
    local tcp_pose = point.tcp_pose or point.pose or point
    tcp_pose = calculateOffsetPoint(tcp_pose)
    local formatted_pose = formatPose(tcp_pose)

    if formatted_pose == nil or #formatted_pose ~= 6 then
        -- 处理 tcp_pose 为 nil 的情况
        log_error(welding_error_ .. "Pose data is empty or dimension mismatch (expected 6D)")
        showErrorMessage("Pose data is empty or dimension mismatch (expected 6D)")
        return
    end

    local run_speed
    if (state == "raw_point_sim") then
        run_speed = params.motion_parameters.tool_speed or 0
    else
        run_speed = speed
    end

    if (run_speed == 0) then
        log_error(welding_error_ .. "moveL speed is 0")
    end

    local blend_radius

    if is_welding_endpoints then
        blend_radius = 0.0
    else
        blend_radius = params.blend_radius or 0.02
    end

    local print_out = string.format("moveLine(%s,%f,%f,%f,0)", formatPoseString(tcp_pose),
        params.motion_parameters.tool_acc or 1.2, run_speed, blend_radius
    )
    log_info(welding_info_ .. print_out)
    moveLine(formatted_pose, params.motion_parameters.tool_acc or 1.2,
        run_speed, blend_radius, 0)
    return print_out
end

local function generateMoveP(point, speed, params, state, is_welding_endpoints)
    local _ENV = sched.select_robot(1)
    local tcp_pose = point.tcp_pose or point.pose or point
    tcp_pose = calculateOffsetPoint(tcp_pose)
    local formatted_pose = formatPose(tcp_pose)
    if formatted_pose == nil or #formatted_pose ~= 6 then
        -- 处理 tcp_pose 为 nil 的情况
        log_error(welding_error_ .. "Pose data is empty or dimension mismatch (expected 6D)")
        showErrorMessage("Pose data is empty or dimension mismatch (expected 6D)")
        return
    end

    local run_speed
    if (state == "raw_point_sim") then
        run_speed = params.motion_parameters.tool_speed or 0
    else
        run_speed = speed
    end

    if (run_speed == 0) then
        log_error(welding_error_ .. "moveP speed is 0")
        return ""
    end

    local blend_radius

    if is_welding_endpoints then
        blend_radius = 0.0
    else
        blend_radius = params.blend_radius or 0.02
    end

    local print_out = string.format("moveProcess(%s,%f,%f,%f)", formatPoseString(tcp_pose),
        params.motion_parameters.tool_acc or 1.2, run_speed,
        blend_radius)
    log_info(welding_info_ .. print_out)
    moveProcess(formatted_pose, params.motion_parameters.tool_acc or 1.2,
        run_speed, blend_radius)
    return print_out
end

local function generateMoveArc(point1, point2, speed, params, state, is_welding_endpoints)
    local _ENV = sched.select_robot(1)

    local point1_pose = point1.tcp_pose or point1.pose or point1
    local point2_pose = point2.tcp_pose or point2.pose or point2

    point1_pose = calculateOffsetPoint(point1_pose)
    point2_pose = calculateOffsetPoint(point2_pose)

    local run_speed
    if (state == "raw_point_sim") then
        run_speed = params.motion_parameters.tool_speed or 0
    else
        run_speed = speed
    end

    if (run_speed == 0) then
        log_error(welding_error_ .. "moveL speed is 0")
    end

    local blend_radius

    if is_welding_endpoints then
        blend_radius = 0.0
    else
        blend_radius = params.blend_radius or 0.02
    end

    local print_out = string.format("moveCircle(%s,%s,%f,%f,%f,0)",
        formatPoseString(point1_pose),
        formatPoseString(point2_pose), params.motion_parameters.tool_acc, run_speed
        , blend_radius)
    log_info(welding_info_ .. "setCirclePathMode(2)")
    setCirclePathMode(2)
    log_info(welding_info_ .. print_out)
    moveCircle(formatPose(point1_pose), formatPose(point2_pose),
        params.motion_parameters.tool_acc, run_speed, blend_radius, 0)
    return print_out
end

-- ==================== 摆动处理 ====================
local function validateWeaveInputParameters(prev_action, points)
    local _ENV = sched.select_robot(1)
    if not prev_action or not prev_action.roadpoints or #prev_action.roadpoints == 0 then
        log_error(welding_error_ .. "Invalid previous action data")
        return false, "Invalid previous action data"
    end
    if not points or #points == 0 then
        log_error(welding_error_ .. "No target points provided")
        return false, "No target points provided"
    end
    return true
end

local function getPoseFromPoint(point)
    return point and (point.pose or point.tcp_pose) or { 0, 0, 0, 0, 0, 0 }
end

local function prepareWeaveParams(motion_param)
    return {
        angle = motion_param.angle or 0,
        speed = (motion_param.welding_speed or 0.0005), -- 默认0.5mm/s
        freq = motion_param.frequency or 1.0,
        hold_dist = (motion_param.hold_distance or 0),
        amp = (motion_param.amplitude or 0.005), -- 默认5mm
        dir = motion_param.direction or 0
    }
end

local function printWeaveParams(params)
    local _ENV = sched.select_robot(1)
    log_info(welding_info_ ..
        string.format("Swing parameters: angle=%f, speed=%f, freq=%f, hold=%f, amp=%f, dir=%d",
            params.angle, params.speed, params.freq, params.hold_dist, params.amp, params.dir))
end

local function processWeavePath(path, motion_param, params, state)
    local _ENV = sched.select_robot(1)
    if not path or #path == 0 then
        log_error(welding_error_ .. "Swing Path is empty")
        return
    end

    log_info(welding_info_ .. string.format("Swing path contains %d points:", #path))

    local sign = true

    local standing_time = motion_param.standing_time or 0.0
    local left_standing_time, right_standing_time

    if standing_time and standing_time >= 0.0 then
        left_standing_time = standing_time
        right_standing_time = standing_time
    else
        left_standing_time = motion_param.left_standing_time or 0.0
        right_standing_time = motion_param.right_standing_time or 0.0
    end

    for i, point in ipairs(path) do
        -- 优化数据验证
        if not point or type(point) ~= "table" or #point < 6 then
            log_error(welding_error_ .. string.format("Point %d: INVALID_DATA (expected 6D vector, got %s)",
                i, point and type(point) or "nil"))
        else
            -- 解构赋值
            local x, y, z, rx, ry, rz = point[1] or 0, point[2] or 0, point[3] or 0,
                point[4] or 0, point[5] or 0, point[6] or 0

            log_info(welding_info_ .. string.format("Point %d: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]",
                i, x, y, z, rx, ry, rz))

            generateMoveLine(point, calculateActualSpeed(motion_param), params, state)

            local current_standing_time = sign and left_standing_time or right_standing_time
            if current_standing_time > 0.0 then
                sleep(current_standing_time)
                log_info(welding_info_ .. string.format("sleep(%.3f)", current_standing_time))
            end
            sign = not sign
        end
    end
    syncProgramPoint()
end

--直线月牙摆动
local function linearCrescentOscillation(motion_param, prev_point, current_point, params, state)
    local _ENV = sched.select_robot(1)
    log_info(welding_info_ .. "Starting pose data used:", table.unpack(prev_point))
    log_info(welding_info_ .. "Target pose data used:", table.unpack(current_point))

    local weave_params = prepareWeaveParams(motion_param)
    printWeaveParams(weave_params)

    local ok, saw_result = pcall(function()
        return aubo_math.getStraightCrescentSwingingPath(
            formatPose(prev_point),
            formatPose(current_point),
            weave_params.angle, weave_params.speed, weave_params.freq,
            weave_params.hold_dist, weave_params.amp, weave_params.dir
        )
    end)


    if not ok or type(saw_result) ~= "table" then
        return "", ErrorCode.COMMON.TABLE_ERROR
    end

    if not saw_result.success then
        return "", saw_result.error or ErrorCode.COMMON.UNKNOWN_ERROR
    end

    if not saw_result.path or #saw_result.path == 0 then
        log_error(welding_error_ .. "Swing Path is empty")
        return "", ErrorCode.COMMON.SUCCESS
    end

    log_info(welding_info_ .. string.format("Swing path contains %d points:", #saw_result.path))

    local path = saw_result.path

    local start_point_swing = path[1]
    local end_point_swing = path[#path]

    local welding_speed = calculateActualSpeed(motion_param)

    generateMoveLine(start_point_swing, welding_speed, params, state)

    local path = saw_result.path

    -- 安全判断
    if not path or #path < 3 then
        log_error(welding_error_ .. "Path must contain at least 3 points for grouping.")
        return "", ErrorCode.COMMON.SUCCESS
    end

    for i = 2, #path - 2, 2 do -- 每次步进 2，保证取连续的 3 个点，且第3点给下一组用
        local point1 = path[i]
        local point2 = path[i + 1]
        local point3 = path[i + 2]

        log_info(welding_info_ .. string.format("Processing group %d: point1=path[%d], point2=path[%d], point3=path[%d]",
            i // 3 + 1, i, i + 1, i + 2))

        -- 安全检查每个点
        if type(point1) ~= "table" or #point1 < 6 then
            log_error(welding_error_ .. string.format("  Point 1 (index %d): INVALID_DATA", i))
        else
            log_info(welding_info_ .. string.format("  Point 1: [%f, %f, %f, %f, %f, %f]",
                point1[1], point1[2], point1[3], point1[4], point1[5], point1[6]))
        end

        if type(point2) ~= "table" or #point2 < 6 then
            log_error(welding_error_ .. string.format("  Point 2 (index %d): INVALID_DATA", i + 1))
        else
            log_info(welding_info_ .. string.format("  Point 2: [%f, %f, %f, %f, %f, %f]",
                point2[1], point2[2], point2[3], point2[4], point2[5], point2[6]))
        end

        if type(point3) ~= "table" or #point3 < 6 then
            log_error(welding_error_ .. string.format("  Point 3 (index %d): INVALID_DATA", i + 2))
        else
            log_info(welding_info_ .. string.format("  Point 3: [%f, %f, %f, %f, %f, %f]",
                point3[1], point3[2], point3[3], point3[4], point3[5], point3[6]))
        end

        if point1 and #point1 >= 6 then
            generateMoveLine(point1, welding_speed, params, state)
        end

        -- 如果有 point3，执行 point2 -> point3 的圆弧（或其他操作）
        if point3 and #point3 >= 6 then
            local standing_time = motion_param.standing_time or 0.0

            if point2 and #point2 >= 6 then
                generateMoveArc(point2, point3, welding_speed, params, state)

                if standing_time > 0.0 then
                    sleep(standing_time)
                    log_info(welding_info_ .. string.format("sleep(%f)", standing_time))
                end
            else
                log_error(welding_error_ .. "Point 2 is invalid for arc.")
            end
        else
            generateMoveLine(point2, welding_speed, params, state)
            log_info(welding_info_ .. "No Point 3 or not enough points: ending group.")
        end
    end
    generateMoveLine(end_point_swing, welding_speed, params, state)

    syncProgramPoint()

    return "", ErrorCode.COMMON.SUCCESS
end

local function handleStraightWeave(motion_param, prev_point, current_point, params, state)
    local _ENV = sched.select_robot(1)

    log_info(welding_info_ .. "Starting pose data used:", table.unpack(prev_point))
    log_info(welding_info_ .. "Target pose data used:", table.unpack(current_point))

    local weave_params = prepareWeaveParams(motion_param)
    printWeaveParams(weave_params)

    local ok, saw_result = pcall(function()
        return aubo_math.getStraightSawtoothSwingPath(
            formatPose(prev_point),
            formatPose(current_point),
            weave_params.angle, weave_params.speed, weave_params.freq,
            weave_params.hold_dist, weave_params.amp, weave_params.dir
        )
    end)

    if not ok or type(saw_result) ~= "table" then
        return "", ErrorCode.COMMON.TABLE_ERROR
    end

    if not saw_result.success then
        showErrorMessage("Straight-tooth swing calculation error")
        return "", saw_result.error or ErrorCode.COMMON.UNKNOWN_ERROR
    end

    processWeavePath(saw_result.path, motion_param, params, state)
    return true
end

local function handleCircularWeave(motion_param, points, params)
    local _ENV = sched.select_robot(1)
    local start_pose = formatPose(points[1])
    local middle_pose = formatPose(points[2])
    local end_pose = formatPose(points[3])

    log_info(welding_info_ .. "Use the starting target pose data:", table.unpack(start_pose))
    log_info(welding_info_ .. "Use intermediate target pose data:", table.unpack(middle_pose))
    log_info(welding_info_ .. "End the use of target pose data:", table.unpack(end_pose))

    local angle = motion_param.angle or 0
    local amplitude = (motion_param.amplitude or 0)

    local step = (motion_param.welding_speed or 0) / (motion_param.frequency or 1)

    log_info(welding_info_ ..
        string.format("Swing parameters: angle=%f, amplitude_inner=%f, amplitude_outer=%f, step=%f",
            angle, amplitude, amplitude, step))

    local ok, saw_result = pcall(function()
        return aubo_math.getCircularArcSawtoothSwingingPath(
            start_pose,
            middle_pose,
            end_pose,
            amplitude, amplitude, step, angle
        )
    end)

    if not ok or type(saw_result) ~= "table" then
        return "", ErrorCode.COMMON.TABLE_ERROR
    end

    if not saw_result.success then
        return "", saw_result.error or ErrorCode.COMMON.UNKNOWN_ERROR
    end

    processWeavePath(saw_result.path, motion_param, params)
    return true
end

-- ==================== 线激光识别路点 ====================
-- 创建一个表来存储数据
-- 现在每个键对应的值是一个包含两组数据的表：{tcp_pose = {...}, joint_angles = {...}}
laser_data_store_ = {}

-- 添加函数：向表中添加键和对应的值（包含tcp_pose和joint_angles两组数据）
-- 参数：key - 键名(如"a")，tcp_pose - TCP位姿数据列表(如{1,2,3,4,5,6})，joint_angles - 关节角数据列表(如{7,8,9,10,11,12})
local function addData(key, tcp_pose, joint_angles)
    -- 检查输入参数是否有效
    if not tcp_pose or not joint_angles then
        return
    end

    -- 存储数据
    laser_data_store_[key] = {
        tcp_pose = tcp_pose,
        joint_angles = joint_angles
    }
end

-- 取用函数：从表中获取指定键的完整数据（包含tcp_pose和joint_angles两组数据）
local function getLaserData(key)
    if not key or type(key) ~= "string" then
        return nil
    end
    local stored_data = laser_data_store_[key]
    log_info(welding_info_ .. "GetLaserData:" .. key)
    if stored_data then
        return stored_data
    end
    return nil
end

-- 清空函数：清空整个表
local function clearData()
    laser_data_store_ = {}
end

--==================== 线激光传感器 ====================

--[[ 连接设备并初始化Modbus信号
    @param device_config 设备配置表，包含连接信息和寄存器地址
     device_config.connection：连接信息表
           connection.ip_port：IP端口号
           connection.slave_num：从机地址
     device_config.addresses：寄存器地址表
           addresses.laser_operate：激光操作寄存器地址
           addresses.job_number：线机光job号寄存器地址
           addresses.target_pose_validity：目标位姿有效性寄存器地址
           addresses.target_pose_in_device_x：设备坐标系下目标位姿X寄存器地址
           addresses.target_pose_in_device_y：设备坐标系下目标位姿Y寄存器地址
           addresses.target_pose_in_device_z：设备坐标系下目标位姿Z寄存器地址
    @return 无返回值
]]
local function connectDevice(device_config)
    local _ENV = sched.select_robot(1)

    local conn = device_config.connection
    local addr = device_config.addresses

    modbusAddSignal(conn.ip_port, conn.slave_num, addr.laser_operate, 3, 'LINE_LASER_OPERATE', false)
    modbusAddSignal(conn.ip_port, conn.slave_num, addr.job_number, 3, 'LINE_LASER_JOB_NUMBER', false)
    modbusAddSignal(conn.ip_port, conn.slave_num, addr.target_pose_validity, 3, 'LINE_LASER_TARGET_POSE_VALIDITY', false)
    modbusAddSignal(conn.ip_port, conn.slave_num, addr.target_pose_in_device_x, 3, 'LINE_LASER_READ_TARGET_POSE_X', false)
    modbusAddSignal(conn.ip_port, conn.slave_num, addr.target_pose_in_device_y, 3, 'LINE_LASER_READ_TARGET_POSE_Y', false)
    modbusAddSignal(conn.ip_port, conn.slave_num, addr.target_pose_in_device_z, 3, 'LINE_LASER_READ_TARGET_POSE_Z', false)
end

--[[ 打开、关闭线激光
    @param operate_value 该值为 1：打开，为 0：关闭
    @return bool true 成功,false 失败
]]
local function operateLaserStatus(operate_value)
    local _ENV = sched.select_robot(1)
    modbusSetOutputSignal('LINE_LASER_OPERATE', operate_value)

    local poll_interval = 0.1
    local timeout_period = 5.0
    local elapsed_time = 0.0
    local current_value = modbusGetSignalStatus('LINE_LASER_OPERATE')

    if current_value == operate_value then
        return true
    end

    while elapsed_time < timeout_period do
        sleep(poll_interval)
        elapsed_time = elapsed_time + poll_interval
        current_value = modbusGetSignalStatus('LINE_LASER_OPERATE')
        if current_value == operate_value then
            return true
        end
    end

    log_info(welding_info_ .. " Set operate value: " .. tostring(operate_value))
    log_info(welding_info_ .. " Current station value: " .. tostring(current_value))
    log_error(welding_error_ .. " Failed to set to " .. tostring(operate_value))

    return false
end

--[[ 设置job号
    @param job_number job号
    @return bool true 设置成功, false 设置失败
]]
local function setJobNumber(job_number)
    local _ENV = sched.select_robot(1)

    modbusSetOutputSignal('LINE_LASER_JOB_NUMBER', job_number)

    local poll_interval = 0.1
    local timeout_period = 5.0
    local elapsed_time = 0.0
    local current_value = modbusGetSignalStatus('LINE_LASER_JOB_NUMBER')

    if current_value == job_number then
        return true
    end

    while elapsed_time < timeout_period do
        sleep(poll_interval)
        elapsed_time = elapsed_time + poll_interval
        current_value = modbusGetSignalStatus('LINE_LASER_JOB_NUMBER')
        if current_value == job_number then
            return true
        end
    end

    log_info(welding_info_ .. " Set job number: " .. tostring(job_number))
    log_info(welding_info_ .. " Current station value: " .. tostring(current_value))
    log_error(welding_error_ .. " Failed to set LINE_LASER_JOB_NUMBER to " .. tostring(job_number))

    return false
end

--[[ 获取当前job号
    @return current_job_number 当前job号，数值由设备定义
]]
local function getJobNumber()
    local _ENV = sched.select_robot(1)
    local current_job_number = modbusGetSignalStatus('LINE_LASER_JOB_NUMBER')
    return current_job_number
end

--[[ 检查焊缝识别是否有效
    @return bool true 能识别到焊缝, false 不能识别到焊缝
]]
local function getTargetPosInDeviceValidity()
    local _ENV = sched.select_robot(1)
    local validity = modbusGetSignalStatus('LINE_LASER_TARGET_POSE_VALIDITY')

    if validity ~= 0 then
        return true
    else
        return false
    end
end

--[[ 将原始数值转换为有符号短整型（16位）
    @param value 待转换的原始数值
    @return number 转换后的有符号短整型值，范围[-32768, 32767]
]]
local function toShort(value)
    local integerValue = math.floor(value)
    return ((integerValue + 32768) % 65536) - 32768
end

--[[ 将整数值转换为带两位小数的实际值
    @param value 待转换的整数值
    @return number 转换后的实际值（除以100000）
]]
local function addDecimalPoint(value)
    return value / 100000.0
end

--[[ 读取线激光传感器识别到的焊缝目标点位置
    @return table {x, y, z, 0, 0, 0} 目标点在激光传感器坐标系下的位姿
     (x, y, z): 位置坐标，单位毫米(m), 后三位固定为0
]]
local function getTargetPosInDevice()
    local _ENV = sched.select_robot(1)

    -- 定义目标位置数组 [x, y, z, rx, ry, rz]，目前只计算 x, y, z
    local target = { 0, 0, 0, 0, 0, 0 }

    -- 从 Modbus 读取原始信号值
    local origin_x = modbusGetSignalStatus('LINE_LASER_READ_TARGET_POSE_X')
    local origin_y = modbusGetSignalStatus('LINE_LASER_READ_TARGET_POSE_Y')
    local origin_z = modbusGetSignalStatus('LINE_LASER_READ_TARGET_POSE_Z')

    -- 检查信号是否有效
    if origin_x == nil or origin_y == nil or origin_z == nil then
        log_error(welding_error_ .. "Failed to read one or more Modbus signals for target pose.")
        return target
    end

    -- 将原始值转换为带小数点的浮点数，并加上激光偏移量（带判空处理）
    local function computeAxisValue(rawSignal, offset)
        if offset == nil then
            -- 如果偏移量为 nil，使用默认值 0.0
            offset = 0.0
        end

        local short_val = toShort(rawSignal)
        local decimal_val = addDecimalPoint(short_val)
        return decimal_val + offset
    end

    -- 安全地获取 laser_offset.x/y/z，避免访问 nil 导致崩溃
    local offset_x = laser_device_config and laser_device_config.laser_offset and laser_device_config.laser_offset.x or
        0.0
    local offset_y = laser_device_config and laser_device_config.laser_offset and laser_device_config.laser_offset.y or
        0.0
    local offset_z = laser_device_config and laser_device_config.laser_offset and laser_device_config.laser_offset.z or
        0.0

    target[1] = computeAxisValue(origin_x, offset_x)
    target[2] = computeAxisValue(origin_y, offset_y)
    target[3] = computeAxisValue(origin_z, offset_z)

    return target
end

--[[ 将线激光传感器坐标系下的点转换到机器人基坐标系
   @param hand_eye_vector {x, y, z, rx, ry, rz} 手眼标定结果
    (x,y,z) 单位：m
   @param target_in_laser {x, y, z, 0, 0, 0} 目标点在激光传感器坐标系下的位姿
    (x,y,z) 单位：m
   @param tcp_pose  {x, y, z, rx, ry, rz} 工具坐标系（TCP）在基坐标系下的位姿
    (x,y,z) 单位：m
   @return target_in_base {x, y, z, rx, ry, rz} 目标点在机器人基坐标系下的位姿
   (x,y,z) 单位：m
]]
local function transformTargetPoseToRobotBase(hand_eye_vector, target_in_laser, tcp_pose)
    local _ENV = sched.select_robot(1)
    -- 激光坐标系 → 工具坐标系
    local target_in_tool = poseTrans(hand_eye_vector, target_in_laser)
    -- 工具坐标系 → 基坐标系
    local target_in_base = poseTrans(tcp_pose, target_in_tool)
    target_in_base[4] = tcp_pose[4]
    target_in_base[5] = tcp_pose[5]
    target_in_base[6] = tcp_pose[6]

    return target_in_base
end

local function waitForCondition(timeout_sec, sleep_time)
    local _ENV = sched.select_robot(1)
    -- 设置默认休眠时间(10毫秒)
    sleep_time = sleep_time or 0.01

    local before_sync_time = realtime.time()
    local elapse = 0

    while elapse < timeout_sec do
        elapse = realtime.time() - before_sync_time
        -- 检查条件
        if getTargetPosInDeviceValidity() then
            return true -- 条件满足
        end
        -- 避免CPU占用过高
        sleep(sleep_time)
    end

    return false -- 超时未满足
end

-- 函数：计算目标位姿并添加到action的laser_point_ids中
-- 参数:
--   action: 包含laser_point_ids的action表
-- 返回值: 无
local function calculateAndAddTargetPose(action)
    local _ENV = sched.select_robot(1)
    -- 1. 获取目标在激光坐标系中的位置
    local target_in_laser = getTargetPosInDevice()

    -- 2. 获取机器人TCP位姿
    local robot_tcp_pose = getTcpPose()

    -- 3. 获取手眼标定向量
    local hand_eye_vector = laser_device_config.hand_eye_calibration_results

    -- 4. 计算目标在机器人基坐标系中的位姿
    local result_tcp_pose = transformTargetPoseToRobotBase(hand_eye_vector, target_in_laser, robot_tcp_pose)
    -- 5. 添加数据到action的laser_point_ids第一个元素的UUID中
    -- 注意: 这里假设action.laser_point_ids[1][2]是有效的UUID字符串
    if action.laser_point_ids and action.laser_point_ids[1] and action.laser_point_ids[1][2] then
        addData(action.laser_point_ids[1][2], result_tcp_pose, getJointPositions())
    else
        -- 错误处理: laser_point_ids结构不符合预期
        log_error(welding_error_ .. "Invalid action.laser_point_ids structure: cannot find UUID for data addition")
    end
end

-- ==================== 多层多道工艺处理 ====================

--[[ 创建直线圆弧路径段
    @param line bool true:直线/false:圆弧
    @param start_pose table 起点位姿
    @param via_pose table 经过点位姿（圆弧需要）
    @param end_pose table 终点位姿
    @return 路径段
]]
local function createSegment(line, start_pose, via_pose, end_pose)
    local seg = mp.LineArcSegment.new()
    seg["line"] = line
    seg["start"] = start_pose
    seg["via"] = via_pose
    seg["end"] = end_pose
    return seg
end

--[[ 创建参考坐标系数据
    @param h_plane bool true:水平/false:垂直
    @param o table o点位姿
    @param p table p点位姿
    @return 参考坐标系数据
]]
local function createFrameData(h_plane, o, p)
    local frame = mp.RefFrameData.new()
    frame["h_plane"] = h_plane
    frame["o"] = o
    frame["p"] = p
    return frame
end

--[[ 创建偏移值
    @param y number
    @param z number
    @param a number
    @param b number
    @return
]]
local function createOffset(y, z, a, b)
    local offset = mp.Offset.new()
    offset["y"] = y
    offset["z"] = z
    offset["a"] = a
    offset["b"] = b
    return offset
end

--[[ 多层多道路径规划
    @param src_segs       路劲规划
    @param mp_type MPType 多层多道类型
    @param auto_frame bool 是否自动设置参考坐标系
    @param blender Blender 过渡
    @param offset Offset 偏移值
    @param auto_opti bool 是否自动优化路径
    @return 路径段集合
]]

local function multiPath(h_plane, src_segs, mp_type, auto_frame, blender, offset, auto_opti, point_o, point_p)
    local _ENV = sched.select_robot(1)
    -- 创建多层多道规划器
    local planner = mp.createPlanner(mp_type)
    if (auto_frame) then
        -- 尝试自动设置参考坐标系
        local ret = planner:trySetRefFrame(src_segs, true)
        if ret ~= 0 then
            log_error(welding_error_ .. "TrySetRefFrame failed")
            showErrorMessage("Automatic setting of op point failed")
        end
        assert(ret == 0, "Automatic setting of op point failed")
    else
        -- 或者 手动设置参考坐标系
        local frame = createFrameData(h_plane, point_o, point_p
        )
        local ret = planner:setRefFrame(frame)
        log_info(welding_info_ ..
            "setRefFrame()  " ..
            "h_plane:" ..
            tostring(h_plane) ..
            "  point_o:" .. formatPoseString(point_p) .. " point_p:" .. formatPoseString(point_p) .. " ret:" .. ret)
        if ret ~= 0 then
            log_error(welding_error_ .. "SetRefFrame failed")
            showErrorMessage("Set refframe failed")
        end
        assert(ret == 0, "setRefFrame failed")
    end

    -- 设置过渡类型
    planner:setBlender(blender)

    -- 是否使能路径自动优化
    planner:setAutoPathOptimization(auto_opti)

    -- 获取多层多道计算结果
    local segs_output = planner:pathOffset(src_segs, offset)
    return segs_output
end


local function createCoordinateOffset(x, y, z, rx, ry, rz)
    local coordinate_offset = aubo_math.CoordinateOffset.new()
    coordinate_offset["x"] = x
    coordinate_offset["y"] = y
    coordinate_offset["z"] = z
    coordinate_offset["rx"] = rx
    coordinate_offset["ry"] = ry
    coordinate_offset["rz"] = ry
    return coordinate_offset
end

-- ==================== 断续焊工艺处理 ====================
local function generateLineIntermittentPath(start_point, end_point, gap, segment_length, mode)
    local _ENV = sched.select_robot(1)

    -- 设置直线参数
    local line = aubo_math.LineSegment.new()
    line.start_point = { start_point[1], start_point[2], start_point[3], start_point[4], start_point[5], start_point[6] }
    line.end_point = { end_point[1], end_point[2], end_point[3], end_point[4], end_point[5], end_point[6] }
    line.arc_on = true

    -- 调用断续焊轨迹生成接口
    local line_result = aubo_math.getLineSegments(line, gap, segment_length, mode)

    for i, seg in ipairs(line_result.segments) do
        print(string.format("Segment %d:", i))
        print("  start_point:", table.unpack(seg.start_point))
        print("  end_point:", table.unpack(seg.end_point))
        print("  arc_on:", seg.arc_on)
    end

    return line_result
end

local function intermittentWeldingProcess(start_point, end_point, inter_mode, weld_len, non_weld_len, motion_param,
                                          params, state, is_welding_endpoints, welder_param)
    local _ENV = sched.select_robot(1)

    --生成断续焊路点
    local path_result = generateLineIntermittentPath(start_point, end_point, non_weld_len, weld_len, inter_mode)

    if not path_result or #path_result.segments == 0 then
        log_error(welding_error_ .. "断续焊路径生成失败（空表）")
        return nil, "路径生成失败"
    end

    local segments = path_result.segments

    -- 打印：路径生成结果
    log_info(welding_info_ .. "=== 断续焊路径生成结果 ===")
    log_info(welding_info_ .. string.format("生成总分段数: %d (含焊接段+空走段)", #segments))
    log_info(welding_info_ .. string.format("路径起点（第一条线段）: %s", formatPoseString(segments[1].start_point)))
    log_info(welding_info_ .. string.format("路径终点（最后一条线段）: %s", formatPoseString(segments[#segments].end_point)))

    -- 获取整个断续焊路径的起点（第一条线段的起点）
    local first_segment = segments[1]
    local first_start_point = first_segment.start_point

    local weld_speed = motion_param.welding_speed or 0.008
    local air_speed = params.motion_parameters.tool_speed or 0.025
    local weave = motion_param.weave or 0 -- 0表示无摆焊

    log_info(welding_info_ .. "=== 运动速度参数 ===")
    log_info(welding_info_ .. string.format("焊接速度: %.3f m/s", weld_speed))
    log_info(welding_info_ .. string.format("空走速度: %.3f m/s", air_speed))
    log_info(welding_info_ .. string.format("焊接端点平滑半径: %.3f m (is_welding_endpoints: %s)",
        is_welding_endpoints and 0.0 or (params.blend_radius or 0.02), tostring(is_welding_endpoints)))

    if weave ~= 0 then
        log_info(welding_info_ .. "=== 摆焊参数 ===")
        local weave_params = prepareWeaveParams(motion_param)
        printWeaveParams(weave_params) -- 摆焊参数打印
    end

    generateMoveLine(first_start_point, air_speed, params, state, is_welding_endpoints)

    -- 执行断续焊（按state区分逻辑）
    local total_segments = #segments

    log_info(welding_info_ .. string.format("=== 开始执行断续焊（状态: %s） ===", state))
    if state == "raw_path_weld" then
        -- 实际焊接状态执行
        for i = 1, total_segments do
            local segment = segments[i]
            local current_waypoint = segment.end_point -- 线段终点作为目标点
            local last_waypoint = segment.start_point  -- 线段起点作为起始点
            local is_weld_segment = segment.arc_on     -- 焊接状态
            local is_last_segment = (i == total_segments)

            log_info(welding_info_ .. string.format("执行分段 %d/%d: 类型=%s, 目标位姿=%s",
                i, total_segments,
                is_weld_segment and "焊接段" or "空走段",
                formatPoseString(current_waypoint)))

            if not is_weld_segment then
                -- 空走段：熄弧，以空走速度移动
                stopArc()
                generateMoveLine(current_waypoint, air_speed, params, state, is_welding_endpoints)
            else
                -- 焊接段：起弧，根据摆焊模式执行
                if weave == 1 or weave == 2 then
                    local weave_param = getSwingParams(motion_param)
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    updateWeldingParametSIntermittent(welder_param)
                    startArc()
                    weaveStart(weave_param)
                    generateMoveLine(current_waypoint, weld_speed, params, state, is_welding_endpoints)
                    weaveEnd()
                elseif weave == 3 then
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    updateWeldingParametSIntermittent(welder_param)
                    startArc()
                    linearCrescentOscillation(motion_param, last_waypoint, current_waypoint, params)
                elseif weave == 4 then
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    updateWeldingParametSIntermittent(welder_param)
                    startArc()
                    handleStraightWeave(motion_param, last_waypoint, current_waypoint, params)
                else
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    updateWeldingParametSIntermittent(welder_param)
                    startArc()
                    generateMoveLine(current_waypoint, weld_speed, params, state, is_welding_endpoints)
                end
            end

            -- 最后一段结束后熄弧
            if is_last_segment then
                stopArc()
                log_info(welding_info_ .. "=== 断续焊执行完成，已熄弧 ===")
            end
        end
    elseif state == "raw_path_sim" then
        -- 模拟状态：仅模拟运动，不起弧/熄弧
        for i = 1, total_segments do
            local segment = segments[i]
            local current_waypoint = segment.end_point
            local last_waypoint = segment.start_point
            local is_weld_segment = segment.arc_on

            log_info(welding_info_ .. string.format("执行模拟分段 %d/%d: 类型=%s, 起点位姿=%s, 目标位姿=%s",
                i, total_segments,
                is_weld_segment and "焊接段（模拟）" or "空走段（模拟）",
                formatPoseString(last_waypoint),
                formatPoseString(current_waypoint)))

            if not is_weld_segment then
                generateMoveLine(current_waypoint, air_speed, params, state, is_welding_endpoints)
            else
                -- 焊接段：起弧，根据摆焊模式执行
                if weave == 1 or weave == 2 then
                    local weave_param = getSwingParams(motion_param)
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    weaveStart(weave_param)
                    generateMoveLine(current_waypoint, weld_speed, params, state, is_welding_endpoints)
                    weaveEnd()
                elseif weave == 3 then
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    linearCrescentOscillation(motion_param, last_waypoint, current_waypoint, params)
                elseif weave == 4 then
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    handleStraightWeave(motion_param, last_waypoint, current_waypoint, params)
                else
                    log_info(welding_info_ .. "摆焊启动，模式=" .. weave)
                    generateMoveLine(current_waypoint, weld_speed, params, state, is_welding_endpoints)
                end
            end
        end
    elseif state == "raw_point_sim" then
        -- 快速模拟：仅移动
        for i = 1, total_segments do
            local segment = segments[i]
            local current_waypoint = segment.end_point
            local is_weld_segment = segment.arc_on

            if not is_weld_segment then
                generateMoveLine(current_waypoint, air_speed, params, state, is_welding_endpoints)
            else
                generateMoveLine(current_waypoint, weld_speed, params, state, is_welding_endpoints)
            end
        end
    else
        log_error(welding_error_ .. "未知状态: " .. tostring(state))
        return nil, "未知状态"
    end

    return true, "执行完成"
end

-- ==================== 指令处理器 ====================
local function processInstruction(inst_type, action, params, state, context, is_welding_endpoints)
    local _ENV = sched.select_robot(1)
    local points = action.roadpoints or {}

    local motion_param = action.motion_param or {}
    local welder_param = action.welder_param or {}
    local prev_action = context and context.prev_action
    local _ENV = sched.select_robot(1)

    -- 运动指令处理
    -- 轴动
    if inst_type == "Robot_MoveJoint" then
        log_info(welding_info_ .. "Processing Robot_MoveJoint instruction")
        if not points or #points < 1 then
            return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
        end
        local valid, err = validatePoint(points[1])
        if not valid then
            return "", err
        end

        return generateMoveJoint(points[1], params), ErrorCode.COMMON.SUCCESS
        --设置行号
    elseif inst_type == "Set_Lable" then
        local line_num = action.index
        setLabel(line_num, "")
        textmsg(trans_[language_]["Run line number"] .. ":" .. line_num)
        return "setLabel", ErrorCode.COMMON.SUCCESS
        -- 轴动到准备点
    elseif inst_type == "Robot_MoveJointToReadyPoint" then
        log_info(welding_info_ .. "Processing Robot_MoveJointToReadyPoint instruction")

        local target_point = {}

        if action.roadpoints[1].type ~= "Common" then
            -- 处理激光路点
            if state == "raw_point_sim" then
                return "", ErrorCode.COMMON.SUCCESS
            end

            local target_uuid = action.roadpoints[1].uuid

            target_point = getLaserData(target_uuid)
        else
            if not points or #points < 1 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end
            local valid, err = validatePoint(points[1])
            if not valid then
                return "", err
            end
            target_point.tcp_pose = points[1].tcp_pose
            target_point.joint_angles = points[1].q
        end

        local offset = { 0, 0, -params.offset_distance or 0.02, 0, 0, 0 }
        local prepare_point = {
            tcp_pose = poseTrans(target_point.tcp_pose, offset),
            q = target_point.joint_angles
        }
        return generateMoveJoint(prepare_point, params), ErrorCode.COMMON.SUCCESS
        -- 直线至准备点
    elseif inst_type == "Robot_MoveLineToReadyPoint" then
        log_info(welding_info_ .. "Processing Robot_MoveLineToReadyPoint instruction")

        local target_point = {}

        if prev_action.roadpoints[1].type ~= "Common" then
            -- 处理激光路点
            if state == "raw_point_sim" then
                return "", ErrorCode.COMMON.SUCCESS
            end

            local target_uuid = prev_action.roadpoints[#prev_action.roadpoints].uuid

            target_point = getLaserData(target_uuid)
        else
            target_point.tcp_pose = prev_action.roadpoints[#prev_action.roadpoints].tcp_pose
            target_point.joint_angles = prev_action.roadpoints[#prev_action.roadpoints].q
        end

        local offset = { 0, 0, -params.offset_distance or 20.0, 0, 0, 0 }

        local prepare_point = {
            tcp_pose = poseTrans(target_point.tcp_pose, offset),
            q = target_point.joint_angles
        }

        return generateMoveLine(prepare_point, params.motion_parameters.tool_speed, params, state),
            ErrorCode.COMMON.SUCCESS
        -- 空走直线
    elseif inst_type == "Robot_AirMoveLine" then
        log_info(welding_info_ .. "Processing Robot_AirMoveLine instruction")

        local target_point = {}

        if action.roadpoints[1].type ~= "Common" then
            -- 处理激光路点
            if state == "raw_point_sim" then
                return "", ErrorCode.COMMON.SUCCESS
            end

            local target_uuid = action.roadpoints[1].uuid

            target_point = getLaserData(target_uuid)
        else
            if not points or #points < 1 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end
            local valid, err = validatePoint(points[1])
            if not valid then
                return "", err
            end
            target_point.tcp_pose = points[1].tcp_pose
            target_point.joint_angles = points[1].q
        end

        return
            generateMoveLine(target_point.tcp_pose, params.motion_parameters.tool_speed, params, state,
                is_welding_endpoints),
            ErrorCode.COMMON.SUCCESS
        -- 直线运动
    elseif inst_type == "Robot_MoveLine" then
        log_info(welding_info_ .. "Processing Robot_MoveLine instruction")

        local target_point = {}

        --激光探寻结束点特殊处理
        if action.roadpoints[1].type ~= "Common" then
            -- 处理激光路点
            if state == "raw_point_sim" then
                return "", ErrorCode.COMMON.SUCCESS
            end

            local target_uuid = action.roadpoints[1].uuid
            target_point = getLaserData(target_uuid)
        else
            if not points or #points < 1 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end
            local valid, err = validatePoint(points[1])
            if not valid then
                return "", err
            end
            target_point.tcp_pose = points[1].tcp_pose
            target_point.joint_angles = points[1].q

            if (state ~= "raw_point_sim" and action.event == 14) then
                handler_laser_thread_move_5 = thread(function()
                    sched.INST([[flag_laser_thread_move_5 = 1]], _ENV)
                    generateMoveLine(target_point.tcp_pose, action.laser_param.laser_until_speed, params, state)
                    sched.INST([[flag_laser_thread_move_5 = 2]], _ENV)
                end, "laser_thread_move_5")
                return "", ErrorCode.COMMON.SUCCESS
            end
        end

        return generateMoveLine(target_point.tcp_pose, motion_param.welding_speed, params, state, is_welding_endpoints),
            ErrorCode.COMMON.SUCCESS
        -- moveP运动
    elseif inst_type == "Robot_MoveP" then
        log_info(welding_info_ .. "Processing Robot_MoveP instruction")

        local target_point = {}

        --激光探寻结束点特殊处理
        if action.roadpoints[1].type ~= "Common" then
            -- 处理激光路点
            if state == "raw_point_sim" then
                return "", ErrorCode.COMMON.SUCCESS
            end

            local target_uuid = action.roadpoints[1].uuid
            target_point = getLaserData(target_uuid)
        else
            if not points or #points < 1 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end
            local valid, err = validatePoint(points[1])
            if not valid then
                return "", err
            end
            target_point.tcp_pose = points[1].tcp_pose
            target_point.joint_angles = points[1].q
        end

        return generateMoveP(target_point.tcp_pose, motion_param.welding_speed, params, state, is_welding_endpoints),
            ErrorCode.COMMON.SUCCESS

        -- 空走圆弧
    elseif inst_type == "Robot_AirMoveArc" then
        if #points < 2 then
            return "", ErrorCode.WAYPOINT.DIMENSION_ERR_WAYPOINT
        end

        local point_2 = {}
        local point_3 = {}

        if action.roadpoints[1].type ~= "Common" then
            -- 处理激光路点
            if state == "raw_point_sim" then
                return "", ErrorCode.COMMON.SUCCESS
            end

            point_2 = getLaserData(action.roadpoints[2].uuid)
            point_3 = getLaserData(action.roadpoints[3].uuid)
        else
            local valid1, err1 = validatePoint(action.roadpoints[2])
            local valid2, err2 = validatePoint(action.roadpoints[3])
            if not valid1 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end
            if not valid2 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end

            point_2 = action.roadpoints[2]
            point_3 = action.roadpoints[3]
        end

        return generateMoveArc(point_2, point_3, params.motion_parameters.tool_speed, params,
            state, is_welding_endpoints), ErrorCode.COMMON.SUCCESS
        -- 圆弧运动
    elseif inst_type == "Robot_MoveArc" then
        if #points < 2 then
            return "", ErrorCode.WAYPOINT.DIMENSION_ERR_WAYPOINT
        end

        local point_2 = {}
        local point_3 = {}

        if action.roadpoints[1].type ~= "Common" then
            -- 处理激光路点
            if state == "raw_point_sim" then
                return "", ErrorCode.COMMON.SUCCESS
            end

            point_2 = getLaserData(action.roadpoints[2].uuid)
            point_3 = getLaserData(action.roadpoints[3].uuid)
        else
            local valid1, err1 = validatePoint(action.roadpoints[2])
            local valid2, err2 = validatePoint(action.roadpoints[3])
            if not valid1 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end
            if not valid2 then
                return "", ErrorCode.WAYPOINT.EMPTY_WAYPOINT
            end

            point_2 = action.roadpoints[2]
            point_3 = action.roadpoints[3]
        end

        return generateMoveArc(point_2, point_3, motion_param.welding_speed, params, state, is_welding_endpoints),
            ErrorCode.COMMON.SUCCESS
        -- 等待到位
    elseif inst_type == "Robot_WaitArrival" then
        if not is_welding_endpoints then
            return "", ErrorCode.COMMON.SUCCESS
        end

        log_info(welding_info_ .. "Processing Robot_WaitArrival instruction")
        syncProgramPoint()
        return "sched.sync_program_point()", ErrorCode.COMMON.SUCCESS
        -- 开启摆动
    elseif inst_type == "Robot_WeaveStart" then
        -- 在 raw_point_sim 状态下跳过摆动指令
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end

        if not motion_param.weave then
            return "", ErrorCode.WEAVE.TYPE_ERR
        end
        if not motion_param.amplitude then
            return "", ErrorCode.WEAVE.AMPLITUDE_ERR
        end
        if not motion_param.frequency then
            return "", ErrorCode.WEAVE.FREQUENCY_ERR
        end
        local weave_param = getSwingParams(motion_param)
        log_info(welding_info_ .. "weave start")
        weaveStart(weave_param)
        return string.format("WeaveStart(%s)", weave_param), ErrorCode.COMMON.SUCCESS

        -- 结束摆动
    elseif inst_type == "Robot_WeaveEnd" then
        -- 在 raw_point_sim 状态下跳过摆动指令
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end
        log_info(welding_info_ .. "weave end")
        weaveEnd()
        return "weaveEnd()", ErrorCode.COMMON.SUCCESS
        -- 焊接指令处理
        -- 起弧
    elseif inst_type == "Welder_ArcOn" then
        if not (state == "raw_path_weld") then
            return "", ErrorCode.COMMON.SUCCESS -- 模拟状态下跳过ArcOn
        end
        log_info(welding_info_ .. "Processing W elder_ArcOn instruction")
        local valid, err = validateWeldingParams(welder_param)
        if not valid then
            return "", err
        end
        startArc()
        return "startArc()", ErrorCode.COMMON.SUCCESS
        -- 熄弧
    elseif inst_type == "Welder_ArcOff" then
        if not (state == "raw_path_weld") then
            return "", ErrorCode.COMMON.SUCCESS
        end
        log_info(welding_info_ .. "Processing Welder_ArcOff instruction")
        stopArc()
        return "stopArc()", ErrorCode.COMMON.SUCCESS
        -- 更新焊机参数
    elseif inst_type == "Welder_UpdateParameter" then
        if not (state == "raw_path_weld") then
            return "", ErrorCode.COMMON.SUCCESS
        end
        log_info(welding_info_ .. "Processing Welder_UpdateParameter instruction")
        updateWeldingParameters(welder_param.current or 0, welder_param.voltage or 0, welder_param.off_voltage or 0,
            welder_param.off_current or 0, welder_param.arc_extinguishing_time or 0, welder_param.delivery_time or 0,
            welder_param.arc_voltage or 0, welder_param.arc_current or 0, welder_param.starting_arc_time or 0)
        return string.format("updateWeldingParameters(%s,%s,%s,%s,%s,%s,%s,%s,%s)", welder_param.current or 0,
            welder_param.voltage or 0, welder_param.off_voltage or 0, welder_param.off_current or 0,
            welder_param.arc_extinguishing_time or 0, welder_param.delivery_time or 0, welder_param.arc_voltage or 0,
            welder_param.arc_current or 0, welder_param.starting_arc_time or 0), ErrorCode.COMMON.SUCCESS
        -- 熄弧 使用上个动作组的熄弧参数
    elseif inst_type == "Welder_ArcOffPra" then
        if not (state == "raw_path_weld") then
            return "", ErrorCode.COMMON.SUCCESS
        end
        log_info(welding_info_ .. "Processing Welder_ArcOffPra instruction")
        local pre_welder_param = prev_action.welder_param or {}
        updateWeldingParameters(pre_welder_param.current or 1, pre_welder_param.voltage or 2,
            pre_welder_param.off_voltage or 3, pre_welder_param.off_current or 4,
            pre_welder_param.arc_extinguishing_time or 0, pre_welder_param.delivery_time or 0,
            pre_welder_param.arc_voltage or 0, pre_welder_param.arc_current or 0, pre_welder_param.starting_arc_time or 0)
        stopArc()
        return "stopArc()", ErrorCode.COMMON.SUCCESS

        -- 摆动指令组
    elseif inst_type == "Robot_MoveWeave" then
        -- 在 raw_point_sim 状态下跳过摆动指令
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end
        -- 验证必需参数
        if not motion_param.weave then
            return "", ErrorCode.WEAVE.TYPE_ERR
        end
        if not motion_param.amplitude then
            return "", ErrorCode.WEAVE.AMPLITUDE_ERR
        end
        if not motion_param.frequency then
            return "", ErrorCode.WEAVE.FREQUENCY_ERR
        end

        -- 计算points数量
        local count = 0
        for _ in pairs(points) do
            count = count + 1
        end
        log_info(welding_info_ .. string.format("count: %d, motion_param.weave: %d", count, motion_param.weave))

        -- 主处理逻辑
        if count == 1 and motion_param.weave == 3 then
            local valid, err = validateWeaveInputParameters(prev_action, points)
            if not valid then
                return "", err
            end
            local lastRoadpoint = prev_action.roadpoints[#prev_action.roadpoints]
            local start_pose = getPoseFromPoint(lastRoadpoint)
            local target_pose = getPoseFromPoint(points[1])
            return linearCrescentOscillation(motion_param, start_pose, target_pose, params)
        elseif count == 1 and motion_param.weave == 4 then
            local valid, err = validateWeaveInputParameters(prev_action, points)
            if not valid then
                return "", err
            end
            local lastRoadpoint = prev_action.roadpoints[#prev_action.roadpoints]
            local start_pose = getPoseFromPoint(lastRoadpoint)
            local target_pose = getPoseFromPoint(points[1])
            return handleStraightWeave(motion_param, start_pose, target_pose, params)
        elseif count == 3 and motion_param.weave == 3 then
            return "", ErrorCode.COMMON.SUCCESS
        elseif count == 3 and motion_param.weave == 4 then
            local swing_points = {}
            table.insert(swing_points, getPoseFromPoint(points[1]))
            table.insert(swing_points, getPoseFromPoint(points[2]))
            table.insert(swing_points, getPoseFromPoint(points[3]))
            return handleCircularWeave(motion_param, swing_points, params)
        else
            return "", ErrorCode.COMMON.UNKNOWN_ERROR
        end

        -- 断续焊指令组
    elseif inst_type == "Robot_MoveStitchWeld" then
        -- 1. 验证是否开启断续焊
        local is_intermittent = motion_param.is_intermittent_welding or false
        if not is_intermittent then
            log_info(welding_info_ .. "断续焊未开启，跳过解析")
            return "", ErrorCode.COMMON.SUCCESS
        end

        -- 2. 验证断续焊核心参数（与提供的参数命名对应）
        local inter_mode = 1                                   -- 工作模式默认为1
        local weld_len = motion_param.intermittent_weld_length -- 长度m
        local non_weld_len = motion_param.intermittent_non_weld_length

        -- 2.1 验证必需参数是否存在
        if weld_len == nil then
            return "", ErrorCode.INTERMITTENT.WELD_LEN_ERR
        end
        if non_weld_len == nil then
            return "", ErrorCode.INTERMITTENT.NON_WELD_LEN_ERR
        end

        -- 2.2 验证参数值有效性
        if weld_len <= 0 or non_weld_len < 0 then
            return "", ErrorCode.INTERMITTENT.PARAM_VALUE_ERR
        end
        if inter_mode < 0 or inter_mode > 1 then
            return "", ErrorCode.INTERMITTENT.MODE_ERR
        end

        log_info(welding_info_ .. "=== 断续焊核心配置参数 ===")
        log_info(welding_info_ .. string.format("断续焊模式: %d (0=先空走后焊接, 1=先焊接后空走)", inter_mode))
        log_info(welding_info_ ..
            string.format("焊接段长度: %.3f m (原始参数: %.1f mm)", weld_len, motion_param.intermittent_weld_length))
        log_info(welding_info_ ..
            string.format("空走段长度: %.3f m (原始参数: %.1f mm)", non_weld_len, motion_param.intermittent_non_weld_length))
        log_info(welding_info_ ..
            string.format("摆焊模式: %d (0=无摆焊, 1=正弦摆焊, 2=螺旋摆焊, 3=月牙摆焊, 4=直线摆焊)", motion_param.weave or 0))

        -- 计算points数量
        local count = 0
        for _ in pairs(points) do
            count = count + 1
        end
        log_info(welding_info_ .. string.format("目标点数量: %d, 摆焊模式: %d", count, motion_param.weave or 0))

        -- 主处理逻辑
        if count == 1 then
            local valid, err = validateWeaveInputParameters(prev_action, points)
            if not valid then
                return "", err
            end
            local lastRoadpoint = prev_action.roadpoints[#prev_action.roadpoints]
            local start_point = getPoseFromPoint(lastRoadpoint)
            local end_point = getPoseFromPoint(points[1])

            log_info(welding_info_ .. "=== 断续焊路径端点 ===")
            log_info(welding_info_ .. string.format("路径起点位姿: %s", formatPoseString(start_point)))
            log_info(welding_info_ .. string.format("路径终点位姿: %s", formatPoseString(end_point)))
            log_info(welding_info_ .. string.format("weld_len (焊接段长度): %.3f m", weld_len))
            log_info(welding_info_ .. string.format("non_weld_len (空走段长度): %.3f m", non_weld_len))
            -- motion_param（运动参数）
            log_info(welding_info_ .. "6. motion_param (运动参数关键字段):")
            log_info(welding_info_ ..
                string.format("   - welding_speed (焊接速度): %.3f m/s", motion_param.welding_speed or 0.0))
            log_info(welding_info_ .. string.format("   - weave (摆焊模式): %d", motion_param.weave or 0))
            log_info(welding_info_ .. string.format("   - amplitude (摆焊幅度): %.3f m", motion_param.amplitude or 0.0))
            log_info(welding_info_ .. string.format("   - frequency (摆焊频率): %.1f Hz", motion_param.frequency or 0.0))
            -- params（全局参数）
            log_info(welding_info_ .. "7. params (全局参数关键字段):")
            log_info(welding_info_ ..
                string.format("   - joint_acc (关节加速度): %.3f rad/s²", params.motion_parameters.joint_acc or 0.0))
            log_info(welding_info_ ..
                string.format("   - joint_speed (关节速度): %.3f rad/s", params.motion_parameters.joint_speed or 0.0))
            log_info(welding_info_ ..
                string.format("   - tool_acc (工具加速度): %.3f m/s²", params.motion_parameters.tool_acc or 0.0))
            log_info(welding_info_ ..
                string.format("   - tool_speed (工具空走速度): %.3f m/s", params.motion_parameters.tool_speed or 0.0))
            log_info(welding_info_ ..
                string.format("   - blend_radius (交融半径): %.3f m", params.motion_parameters.blend_radius or 0.0))
            -- state（运行状态：如raw_path_sim/raw_path_weld）
            log_info(welding_info_ .. string.format("state (运行状态): %s", state or "未知状态"))
            -- is_welding_endpoints（端点平滑标记）
            log_info(welding_info_ ..
                string.format("is_welding_endpoints (端点平滑标记): %s", tostring(is_welding_endpoints or false)))
            log_info(welding_info_ .. "===============================================")

            intermittentWeldingProcess(start_point, end_point, inter_mode, weld_len, non_weld_len, motion_param, params,
                state, is_welding_endpoints, welder_param)
            return "", ErrorCode.COMMON.SUCCESS
        else
            return "", ErrorCode.COMMON.UNKNOWN_ERROR
        end
        --打开线激光
    elseif inst_type == "LineLaser_Open" then
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end
        log_info(welding_info_ .. "Open laser")

        --开启激光
        if (not operateLaserStatus(1)) then
            log_info(welding_info_ .. "Failed to activate laser")
            return "", ErrorCode.LINELASER.OPEN_LASER_ERR
        end
        --设置任务号
        if (action.event == 12 or action.event == 14) then
            local task_number = action.laser_param.laser_channel
            log_info(welding_info_ .. "Task_number:" .. task_number)
            log_info(welding_info_ .. "GetJobNumber:" .. getJobNumber())
            if (task_number ~= getJobNumber() and not setJobNumber(task_number)) then
                return "", ErrorCode.LINELASER.SET_TEMPLATE_ERR
            end
        end
        return "", ErrorCode.COMMON.SUCCESS

        --关闭线激光
    elseif inst_type == "LineLaser_Close" then
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end
        log_info(welding_info_ .. "Close laser")
        --关闭激光
        if (not operateLaserStatus(0)) then
            log_info(welding_info_ .. "Laser shutdown failed")
            return "", ErrorCode.LINELASER.CLOSE_LASER_ERR
        end
        return "", ErrorCode.COMMON.SUCCESS
        --开始采集数据
    elseif inst_type == "Collect_Start" then
        return "", ErrorCode.COMMON.SUCCESS
        --停止采集数据
    elseif inst_type == "Collect_Stop" then
        return "", ErrorCode.COMMON.SUCCESS
    elseif inst_type == "Sleep" then
        return "", ErrorCode.COMMON.SUCCESS
        --等待到位或识别到焊缝
    elseif inst_type == "WaitTimeoutOrFound" or inst_type == "Robot_WaitArrivalOrFound" then
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end

        if (action.event == 12) then
            syncProgramPoint()
            local timeout_sec = action.laser_param.laser_seek_holdtime
            waitForCondition(timeout_sec)
        elseif (action.event == 14) then
            log_info(welding_info_ .. "Start exploring")
            flag_laser_thread_move_5 = 0
            run(handler_laser_thread_move_5)
            local hit_0 = 0
            while flag_laser_thread_move_5 ~= 2 do
                sleep(0.03)
                if flag_laser_thread_move_5 == 1 and getTargetPosInDeviceValidity() == true then
                    hit_0 = hit_0 + 1
                else
                    hit_0 = 0
                end
                if hit_0 > 5 then
                    stopLine(0.1)
                    hit_0 = 0
                    flag_laser_thread_move_5 = 2
                    break
                end
                sync()
            end
            kill(handler_laser_thread_move_5)

            if (not getTargetPosInDeviceValidity()) then
                local timeout_sec = laser_device_config.explore_waiting_time
                waitForCondition(timeout_sec)
            end
        end
        --循环结束到焊缝
        if (getTargetPosInDeviceValidity()) then
            log_info(welding_info_ .. "Identified weld seam")
            calculateAndAddTargetPose(action)
        else
            showErrorMessage("No weld seam identified")
            log_error(welding_error_ .. "No weld seam identified")
            abort()
        end
        return "", ErrorCode.COMMON.SUCCESS
        -- 计算交叉线
    elseif inst_type == "Calculate_Intersection" then
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end


        local intersection_ref_lines = action.intersection_ref_lines
        local temp_offset = createCoordinateOffset(0, 0, 0, 0, 0, 0)

        local ref_line_1 = intersection_ref_lines["ref_line1"]
        local p1_uuid = ref_line_1[1]
        local p2_uuid = ref_line_1[2]

        local ref_line_2 = intersection_ref_lines["ref_line2"]
        local p3_uuid = ref_line_2[1]
        local p4_uuid = ref_line_2[2]

        local p1_point = getLaserData(p1_uuid).tcp_pose
        local p2_point = getLaserData(p2_uuid).tcp_pose
        local p3_point = getLaserData(p3_uuid).tcp_pose
        local p4_point = getLaserData(p4_uuid).tcp_pose

        log_info("========交叉计算点========")
        log_info(welding_info_ .. formatPoseString(p1_point))
        log_info(welding_info_ .. formatPoseString(p2_point))
        log_info(welding_info_ .. formatPoseString(p3_point))
        log_info(welding_info_ .. formatPoseString(p4_point))
        log_info("==========================")

        -- 直线1
        local line1 = { p1_point, p2_point }
        -- 直线2
        local line2 = { p3_point, p4_point }
        -- 调用函数
        local coord_vector = aubo_math.buildCoordinateVector(line1, line2, temp_offset)
        log_info(welding_info_ .. "coord_vector:" .. formatPoseString(coord_vector))
        local q = getJointPositions()

        if action.laser_point_ids and action.laser_point_ids[1] and action.laser_point_ids[1][2] then
            addData(action.laser_point_ids[1][2], coord_vector, q)
        else
            -- 错误处理: laser_point_ids结构不符合预期
            log_error(welding_error_ .. "Invalid action.laser_point_ids structure: cannot find UUID for data addition")
        end

        if action.laser_point_ids and action.laser_point_ids[2] and action.laser_point_ids[2][2] then
            addData(action.laser_point_ids[2][2], coord_vector, q)
            local str = string.format("{%f,%f,%f,%f,%f,%f}|{%f,%f,%f,%f,%f,%f}", coord_vector[1], coord_vector[2],
                coord_vector[3], coord_vector[4],
                coord_vector[5], coord_vector[6], q[1], q[2], q[3], q[4], q[5], q[6])
            popup(TraceLevel.INFO, action.laser_point_ids[2][2], str, 2001)
        end
        return "", ErrorCode.COMMON.SUCCESS
        --计算偏移点
    elseif inst_type == "Calculate_Offset" then
        if state == "raw_point_sim" then
            return "", ErrorCode.COMMON.SUCCESS
        end

        local offset_ref_pts = action.offset_ref_pts

        local p1_uuid = offset_ref_pts["ref_pt1"]
        local p2_uuid = offset_ref_pts["ref_pt2"]

        local p1 = getLaserData(p1_uuid)
        local p2 = getLaserData(p2_uuid)
        log_info(welding_info_ .. "ref_pt1-uuid" .. p1_uuid)
        log_info(welding_info_ .. "ref_pt2-uuid" .. p2_uuid)


        local p1_tcp_pose = {}
        local p2_tcp_pose = {}

        if p1 then
            p1_tcp_pose = p1.tcp_pose
        else
            p1 = laser_points_db_[offset_ref_pts["ref_pt1"]]
            if p1 then
                p1_tcp_pose = p1["tcp_pose"]
            else
                log_error(welding_error_ .. "Search for offset point p1 failed")
                return "", ErrorCode.COMMON.SUCCESS
            end
        end

        if p2 then
            p2_tcp_pose = p2.tcp_pose
        else
            p2 = laser_points_db_[offset_ref_pts["ref_pt2"]]
            if p2 then
                p2_tcp_pose = p2[tcp_pose]
            else
                log_error(welding_error_ .. "Search for offset point p2 failed")
                return "", ErrorCode.COMMON.SUCCESS
            end
        end

        global_offset_ = { true, p1_tcp_pose, p2_tcp_pose }
        return "", ErrorCode.COMMON.SUCCESS
    else
        log_info(welding_info_ .. "Unsupported instruction type:", inst_type)
        return "", ErrorCode.COMMON.SUCCESS
    end
end

local function multiProcessProcessing(multi_channel_data, params, state)
    -- 设置机器人环境
    local _ENV = sched.select_robot(1)
    local instructions = {}

    -- 验证多层焊接数据格式
    local first = multi_channel_data[1]
    local last = multi_channel_data[#multi_channel_data]
    if first.action_.event ~= 8 or last.action_.event ~= 9 then
        log_error(welding_error_ ..
            "Multi channel data does not meet the requirements, it is not the beginning and end of multiple channels")
        return {}
    end

    -- 提取有效动作（跳过开始和结束动作）
    local actions = {}
    for i = 2, #multi_channel_data - 1 do
        table.insert(actions, multi_channel_data[i].action_)
    end

    -- 多层多道处理线激光点位
    for i, action in ipairs(actions) do
        if action.roadpoints[1].type ~= "Common" then
            local _ENV = sched.select_robot(1)
            for j, rp in ipairs(action.roadpoints) do
                local data = getLaserData(rp.uuid)
                local real_q, err = inverseKinematics(data.joint_angles, data.tcp_pose)
                if err == 0 then
                    rp.tcp_pose = data.tcp_pose
                    rp.q = real_q
                else
                    showErrorMessage("InverseKinematics failed for laser point")
                    return
                end
            end
        end
    end

    -- 处理反向模式
    local actions_return = {}
    local multi_layer_startup = first.action_

    if multi_layer_startup.multi_channel_parameters and multi_layer_startup.multi_channel_parameters.mp_is_return_mode then
        -- 反向插入元素
        for i = #actions, 1, -1 do
            local action = actions[i]
            if action.event == 2 then
                -- 圆弧点位交换
                local actions_temporary = deepCopy(action)
                actions_temporary.roadpoints[1] = action.roadpoints[3]
                actions_temporary.roadpoints[2] = action.roadpoints[2]
                actions_temporary.roadpoints[3] = action.roadpoints[1]
                table.insert(actions_return, actions_temporary)
            else
                -- 非圆弧事件直接推入
                table.insert(actions_return, action)
            end
        end
    end

    -- 获取多层多道参数
    local multi_layer_configuration = multi_layer_startup.multi_channel_parameters

    -- 准备焊接路径
    local src_segs = {}
    local old_start_point = {}
    local mp_is_return_mode = multi_layer_configuration.mp_is_return_mode

    -- 检查actions是否有效
    if not actions or #actions == 0 then
        log_error(welding_error_ .. "No actions available for processing")
        return {}
    end

    local execute_action = actions

    if #multi_layer_configuration.layers_parameter.channel_parameter == 0 then
        showErrorMessage("Multiple channels are empty")
        return
    end

    -- 遍历每一道
    for w = 1, #multi_layer_configuration.layers_parameter.channel_parameter do
        log_info(welding_info_ .. "Start traversing the path " .. w)
        local channel_parameter = multi_layer_configuration.layers_parameter.channel_parameter[w]
        if channel_parameter.enable == false then
            goto continue
        end
        syncProgramPoint()

        if (mp_is_return_mode) then
            if w % 2 == 1 then
                execute_action = actions
            else
                execute_action = actions_return
            end
        end

        -- 处理每个动作
        for i = 1, #execute_action do
            local action_data = execute_action[i]
            if not action_data then
                log_error(welding_error_ .. "Action at index " .. i .. " has no action_ field")
                goto continue
            end

            -- 检查是否有下一个元素
            local next_action = (i < #execute_action) and execute_action[i + 1] or nil

            if (i == #execute_action or action_data.event == 0) and #src_segs ~= 0 then
                -- 处理 event 0
                local blender = mp.Blender.new()
                blender["type"] = mp.BlendType.Auto
                blender["blend_radius"] = multi_layer_configuration.layers_parameter.transition_radius

                -- 计算偏移量
                local offset = createOffset(channel_parameter.y, channel_parameter.z,
                    channel_parameter.A, channel_parameter.B)

                -- 确定焊接类型
                local MP_Type = (multi_layer_configuration.layers_parameter.path_weldtype == 0) and
                    mp.MPType.PA_BUTT or mp.MPType.PB_FILLET

                local h_plane = multi_layer_configuration.mp_is_h_plane
                local auto_frame = multi_layer_configuration.mp_is_auto_mode
                local auto_optimize = multi_layer_configuration.mp_is_auto_optimize
                -- 自动设置op点
                local segs
                if auto_frame then
                    segs = multiPath(h_plane, src_segs, MP_Type, auto_frame, blender, offset, auto_optimize, {}, {})
                else
                    local o_point_ok = inverseSolutionSuccess(multi_layer_configuration.mp_o_point)
                    local p_point_ok = inverseSolutionSuccess(multi_layer_configuration.mp_p_point)

                    if not o_point_ok or not p_point_ok then
                        if not o_point_ok then
                            showErrorMessage("Inverse solution failed for mp_o_point")
                            log_error(welding_error_ .. "Inverse solution failed for mp_o_point")
                        end
                        if not p_point_ok then
                            showErrorMessage("Inverse solution failed for mp_p_point")
                            log_error(welding_error_ .. "Inverse solution failed for mp_p_point")
                        end
                        return false -- 或处理错误
                    end

                    segs = multiPath(h_plane, src_segs, MP_Type, auto_frame, blender, offset, auto_optimize,
                        multi_layer_configuration.mp_o_point.tcp_pose,
                        multi_layer_configuration.mp_p_point.tcp_pose)
                end
                src_segs = {} -- 直接覆盖原表，清空所有数据
                old_start_point = {}

                -- 获取焊接速度
                local motion = channel_parameter.craft_param.motion
                local speed = motion.welding_speed or 0
                local welder_param = channel_parameter.craft_param.welder

                if (#segs ~= 0) then
                    generateMoveLine(formatPose(segs[1]["start"]), params.motion_parameters.tool_speed, params, state)
                    syncProgramPoint()
                end

                if state == "raw_path_weld" and #segs ~= 0 then
                    --更新焊接参数
                    updateWeldingParameters(welder_param.current or 0, welder_param.voltage or 0,
                        welder_param.off_voltage or 0,
                        welder_param.off_current or 0, welder_param.arc_extinguishing_time or 0,
                        welder_param.delivery_time or 0, welder_param.arc_voltage or 0,
                        welder_param.arc_current or 0, welder_param.starting_arc_time or 0)
                    --起弧
                    startArc()
                end

                if (state ~= "raw_point_sim" and (motion.weave == 1 or motion.weave == 2)) then
                    local weave_param = getSwingParams(motion)
                    log_info(welding_info_ .. "weave start")
                    weaveStart(weave_param)
                end

                if state ~= "raw_point_sim" and motion.weave == 4 then
                    for seg_idx = 1, #segs do
                        local seg = segs[seg_idx]
                        if seg["line"] then
                            handleStraightWeave(motion, formatPose(seg["start"]), formatPose(seg["end"]), params)
                        else
                            local points = {}
                            table.insert(points, seg["start"])
                            table.insert(points, seg["via"])
                            table.insert(points, seg["end"])
                            handleCircularWeave(motion, points, params)
                        end
                    end
                elseif state ~= "raw_point_sim" and motion.weave == 3 then
                    for seg_idx = 1, #segs do
                        local seg = segs[seg_idx]
                        if seg["line"] then
                            linearCrescentOscillation(motion, formatPose(seg["start"]), formatPose(seg["end"]), params)
                        else
                            local points = {}
                            table.insert(points, seg["start"])
                            table.insert(points, seg["via"])
                            table.insert(points, seg["end"])
                            --圆弧月牙摆动
                        end
                    end
                else
                    -- 生成移动指令
                    for seg_idx = 1, #segs do
                        local seg = segs[seg_idx]
                        local is_last_seg = (seg_idx == #segs)

                        if seg["line"] then
                            generateMoveLine(formatPose(seg["end"]), speed, params, state, is_last_seg)
                        else
                            generateMoveLine(formatPose(seg["start"]), speed, params, state)
                            generateMoveArc(
                                formatPose(seg["via"]),
                                formatPose(seg["end"]),
                                speed,
                                params,
                                state,
                                is_last_seg
                            )
                        end
                    end
                end

                --熄弧
                if state == "raw_path_weld" and #segs ~= 0 then
                    stopArc()
                end
                if (state ~= "raw_point_sim" and (motion.weave == 1 or motion.weave == 2)) then
                    log_info(welding_info_ .. "weave end")
                    weaveEnd()
                end

                -- 添加最后一段移动
                if action_data.event == 0 and action_data.roadpoints and #action_data.roadpoints > 0 then
                    generateMoveLine(formatPose(action_data.roadpoints[1].pose or action_data.roadpoints[1].tcp_pose),
                        params.motion_parameters.tool_speed,
                        params, state)
                else
                    log_error(welding_error_ .. "Action_data at index " .. i .. " has no roadpoints")
                end
            elseif action_data.event == 0 then
                if action_data.roadpoints and #action_data.roadpoints > 0 then
                    generateMoveLine(formatPose(action_data.roadpoints[1].pose or action_data.roadpoints[1].tcp_pose),
                        params.motion_parameters.tool_speed,
                        params, state)
                else
                    log_error(welding_error_ .. "Action_data at index " .. i .. " has no roadpoints")
                end
            elseif action_data.event == 1 and next_action then
                -- 处理 event 1
                if next_action.event == 1 then
                    local start_point = action_data.roadpoints and #action_data.roadpoints > 0 and
                        action_data.roadpoints[1].pose or action_data.roadpoints[1].tcp_pose or
                        nil
                    local end_point = next_action.roadpoints and #next_action.roadpoints > 0 and
                        next_action.roadpoints[1].pose or next_action.roadpoints[1].tcp_pose or
                        nil


                    if start_point and end_point then
                        table.insert(src_segs, createSegment(true, start_point, {}, end_point))
                    else
                        log_error(welding_error_ .. "Missing roadpoints for event 1 processing")
                    end
                end
            elseif action_data.event == 2 then
                -- 处理 event 2
                local point1 = action_data.roadpoints and #action_data.roadpoints > 0 and action_data.roadpoints[1].pose or
                    action_data.roadpoints[1].tcp_pose or
                    nil

                if false == arePointsSimilar(old_start_point, point1) then
                    old_start_point = point1
                    local point2 = action_data.roadpoints and #action_data.roadpoints > 1 and
                        action_data.roadpoints[2].pose or action_data.roadpoints[2].tcp_pose or nil
                    local point3 = action_data.roadpoints and #action_data.roadpoints > 2 and
                        action_data.roadpoints[3].pose or action_data.roadpoints[3].tcp_pose or nil

                    if point1 and point2 and point3 then
                        table.insert(src_segs, createSegment(false, point1, point2, point3))
                    else
                        log_error(welding_error_ .. "Missing roadpoints for event 2 processing")
                    end
                end
            end

            ::continue::
        end
        ::continue::
    end

    return instructions
end

-- ==================== 核心执行逻辑 ====================
M.generateAction = function(data, state, sharedParams, language, enable_laser)
    local _ENV = sched.select_robot(1)
    log_info(welding_info_ .. "==== Starting generateAction ====")
    if (enable_laser) then
        laser_device_config = json.decode(_G.laser_device_config_)
        connectDevice(laser_device_config)
    end

    language_ = language or "en"

    if type(getArcIoValue) ~= "function" then
        initWeldingConfiguration(0, 0, 3, 1, 2, 0, 1)
    end

    local action_command, err = json.decode(data)
    if not action_command then
        log_error(welding_error_ .. "JSON decode failed:", err)
        return false, {
            error_code = 1001,
            error = "JSON decode failed: " .. tostring(err)
        }
    end

    local instructions = action_command[1]
    laser_points_db_ = action_command[2]


    log_info(welding_info_ .. "Decoded instructions count:", #instructions)

    -- 状态验证
    if not (state == "raw_path_weld" or state == "raw_path_sim" or state == "raw_point_sim") then
        log_error(welding_error_ .. "Incorrect running status :" .. state)
        return false, {
            error_code = 1002,
            error = "Invalid welding state"
        }
    end

    -- 设置默认参数
    local params = {
        motion_parameters = {
            joint_acc = sharedParams and sharedParams.joint_acc or 80.0,
            joint_speed = sharedParams and sharedParams.joint_speed or 60.0,
            tool_acc = sharedParams and sharedParams.tool_acc or 1200.0,
            tool_speed = sharedParams and sharedParams.tool_speed or 250.0
        },
        blend_radius = sharedParams and sharedParams.blend_radius or 20.0,
        offset_distance = sharedParams and sharedParams.offset_distance or 20.0,
        back_off_distance = sharedParams and sharedParams.back_off_distance or 20.0
    }

    local results = {}
    local collected_instructions = {} -- 用于存储收集到的指令
    local is_collecting = false       -- 标记是否正在收集

    -- 处理每条指令
    for i, instr in ipairs(instructions) do
        -- 结构验证
        if not (instr.type_ and instr.action_) then
            log_info(welding_info_ .. "Missing type_ or action_ in instruction", i)
            return false, {
                error_code = 2001,
                error = "Missing type_ or action_ field",
                instruction_index = i
            }
        end

        --多道开始
        if instr.action_.event == 8 then
            syncProgramPoint()
            is_collecting = true
            collected_instructions = {} -- 清空之前的收集结果
            table.insert(collected_instructions, instr)
            goto continue               -- 跳过当前指令的后续处理
        end

        -- 多道结束
        if instr.action_.event == 9 then
            if is_collecting then
                table.insert(collected_instructions, instr)
                is_collecting = false

                log_info(welding_info_ .. "Start multiple paths")
                syncProgramPoint()
                multiProcessProcessing(collected_instructions, params, state)
                log_info(welding_info_ .. "End multiple paths")
                -- 跳过后续处理
                goto continue
            else
                log_error(welding_error_ .. "Found MultiLayerClosure without matching startup", i)
                return false, {
                    error_code = 2002,
                    error = "MultiLayerClosure without matching startup",
                    instruction_index = i
                }
            end
        end

        -- 如果正在收集，则添加到收集列表
        if is_collecting then
            table.insert(collected_instructions, instr)
            goto continue -- 跳过当前指令的后续处理
        end

        -- 获取前后action数据
        local instr = instructions[i]
        if not instr then
            return
        end -- 当前指令不存在，直接返回

        -- 当前指令的 action 和 event
        local current_action = instr.action_
        local current_event = current_action and current_action.event or nil

        -- 前后指令的 action
        local prev_action = (i > 1) and instructions[i - 1].action_ or nil
        local next_action = (i < #instructions) and instructions[i + 1].action_ or nil

        -- 是否在序列开始/结束（无前/后指令）
        local is_at_start = (prev_action == nil)
        local is_at_end = (next_action == nil)

        -- 当前是否是焊接动作（直线1、圆弧2、包角18）
        local is_welding_action = (current_event == 1 or current_event == 2 or current_event == 18)

        -- 前一个指令是否为非焊接动作（或不存在）
        local is_prev_non_welding = is_at_start or
            (prev_action and (prev_action.event ~= 1 and prev_action.event ~= 2 and prev_action.event ~= 18))

        -- 后一个指令是否为非焊接动作（或不存在）
        local is_next_non_welding = is_at_end or
            (next_action and (next_action.event ~= 1 and next_action.event ~= 2 and next_action.event ~= 18))

        -- 最终判断：当前是焊接动作，且前后至少有一个是非焊接或边界 → 是焊接端点
        local is_welding_endpoints = is_welding_action and (is_prev_non_welding or is_next_non_welding)

        log_info(welding_info_ .. "Is_welding_endpoints", is_welding_endpoints)

        -- 处理子指令序列
        for subIdx, inst_type in ipairs(instr.type_) do
            local context = {
                prev_action = prev_action,
                next_action = next_action,
                instructionIndex = i,
                subInstructionIndex = subIdx
            }

            local cmd, err = processInstruction(inst_type, instr.action_, params, state, context, is_welding_endpoints)

            if err and err ~= ErrorCode.COMMON.SUCCESS then
                log_error(welding_error_ .. "Instruction processing failed:", err)
                return false, {
                    error_code = 3000 + (MetaInstructionType[inst_type] or 0),
                    error = err,
                    instruction_index = i,
                    sub_instruction_index = subIdx
                }
            end

            if cmd then
                table.insert(results, {
                    seq = i,
                    sub_seq = subIdx,
                    type = inst_type,
                    command = cmd,
                    timestamp = os.time()
                })
            else
                log_error(welding_error_ .. "No command generated for instruction type:", inst_type)
            end
        end
        ::continue::
    end

    log_info(welding_info_ .. "Generated commands count:" .. #results)
    return true, {
        state = state,
        commands = results,
        motion_parameters = params.motion_parameters,
        process_parameters = {
            offset_distance = params.offset_distance,
            blend_radius = params.blend_radius
        },
        start_time = os.time(),
        end_time = os.time()
    }
end

M.emergencyStopArcExtinguishing = function()
    local _ENV = sched.select_robot(1)
    setArcIoOutput(false)
    log_info(welding_info_ .. "setArcIoOutput:" .. "false")
    setGasSolenoidState(false)
    log_info(welding_info_ .. "setGasSolenoidState:" .. "false")
    setTargetCurrentOutput(0)
    log_info(welding_info_ .. "setTargetCurrentOutput:" .. 0)
    setTargetVoltageOutput(0)
    log_info(welding_info_ .. "setTargetVoltageOutput:" .. 0)
end

return M

