-- 单外部轴脚本，不包含示教器生成的程序结构。

local group = "轴组_1"
-- false: 使用系统里已有轴组；true: 脚本启动时重新配置单外部轴轴组。
local configure_axis_group = true

if configure_axis_group then
    local fake_axis1 = app.api:getAxisInterface("fake_axis1")
    fake_axis1:setExtAxisType(1)
    fake_axis1:setExtAxisMaxPositionLimit(500)
    fake_axis1:setExtMinPositionLimit(-500)
    fake_axis1:setExtAxisMaxVelocityLimit(1000)
    fake_axis1:setExtAxisMaxAccelerationLimit(1000)
    fake_axis1:setReductionRatio(1)

    axisGroupDelete(group)
    axisGroupAdd(group, {0, 0, 0}, "base")
    axisGroupAddAxis(group, "fake_axis1", group, {0, 1, 0})
    enableAxisGroup(group)
end

local function table_text(t)
    if type(t) ~= "table" then
        return tostring(t)
    end
    return table.concat(t, ", ")
end

local function require_vector(name, t, size)
    if type(t) ~= "table" or #t < size then
        error(name .. " invalid, expected size >= " .. tostring(size), 2)
    end
    return t
end

local function joint_deg_to_rad(joint)
    local q = {}
    for i = 1, 6 do
        q[i] = math.rad(joint[i])
    end
    return q
end

local function mm_to_m(x)
    return x / 1000.0
end

local function target_axis_mm(name, target)
    if target.axis_mm ~= nil then
        return target.axis_mm
    end
    if target.axis_y ~= nil then
        return target.axis_y
    end
    if target.axis_xyz ~= nil then
        return require_vector(name .. ".axis_xyz", target.axis_xyz, 3)[2]
    end
    return require_vector(name .. ".axis_q", target.axis_q, 1)[1]
end

local function require_target(name, target)
    if type(target) ~= "table" then
        error(name .. " invalid, expected target table", 2)
    end
    local joint = target.joint
    local axis_mm
    if joint == nil then
        require_vector(name, target, 7)
        joint = {target[1], target[2], target[3], target[4], target[5], target[6]}
        axis_mm = target[7]
    else
        require_vector(name .. ".joint", joint, 6)
        axis_mm = target_axis_mm(name, target)
    end
    return {
        joint = joint_deg_to_rad(joint),
        axis_q = {mm_to_m(axis_mm)}
    }
end

local function require_ok(name, ret)
    if ret ~= nil and ret < 0 then
        error(name .. " failed, ret=" .. tostring(ret), 2)
    end
end

local function text_table(prefix, t)
    textmsg(prefix, table_text(t))
end

local function show_target(tag, pose, target)
    textmsg("---- target ", tag, " ----")
    text_table("target_joint_rad: ", target.joint)
    text_table("target_tcp_axis_group: ", pose)
    text_table("target_axis_q_m: ", target.axis_q)
end

local function target_pose(tag, target)
    local pose, fk_ret =
        forwardKinematicsWithAxisGroup(target.joint, target.axis_q)
    require_ok("forwardKinematicsWithAxisGroup " .. tag, fk_ret)
    return require_vector(tag .. ".pose", pose, 6)
end

local joint_a = 0.8
local joint_v = 0.8
local line_a = 0.5
local line_v = 0.3

local DEFAULT_WEAVE_PARAMS = [=[
{
    "type": "SINE",
    "step": 0.003,
    "frequency": 5.0,
    "amplitude": [0.005, 0.005],
    "hold_distance": [0, 0],
    "hold_time": [0, 0],
    "angle": [0, 0],
    "direction": 0,
    "movep_uniform_vel": false
}
]=]

local function start_line_weave()
    local ok, ret = pcall(function()
        return weaveStart(DEFAULT_WEAVE_PARAMS)
    end)
    if not ok then
        textmsg("weaveStart 调用失败: ", tostring(ret))
        return false
    end
    textmsg("weaveStart 返回值: ", tostring(ret))
    return true
end

local function stop_line_weave()
    local ok, ret = pcall(function()
        return weaveEnd()
    end)
    if not ok then
        textmsg("weaveEnd 调用失败: ", tostring(ret))
        return false
    end
    textmsg("weaveEnd 返回值: ", tostring(ret))
    return true
end

-- 变量功能需提供五个目标：前6个为deg关节，后1个为Y向外部轴mm，内部转m。

local origin = require_target("origin_target", origin_target)
local pose1 = require_target("pose_1", pose_1 or pose_1_target or line_target1)
local pose2 = require_target("pose_2", pose_2 or pose_2_target or line_target2)
local pose2_back = require_target("pose_2_back",
                                  pose_2_back or pose_2_back_target)
local back = require_target("back", back or back_target or return_target)

local origin_pose = target_pose("origin", origin)
show_target("origin", origin_pose, origin)
moveJointWithAxisGroup(origin.joint, joint_a, joint_v, 0, 0, group,
                       origin.axis_q)
sleep(0)

local pose1_tcp = target_pose("pose_1", pose1)
show_target("pose_1", pose1_tcp, pose1)
moveJointWithAxisGroup(pose1.joint, joint_a, joint_v, 0, 0, group,
                       pose1.axis_q)
sleep(0)

local pose2_tcp = target_pose("pose_2", pose2)
show_target("pose_2", pose2_tcp, pose2)
if not start_line_weave() then
    textmsg("weaveStart 失败，跳过直线运动")
    return
end
local line_ok, line_err = pcall(function()
    moveLineWithAxisGroup(pose2_tcp, line_a, line_v, 0, 0, group,
                          pose2.axis_q)
end)
stop_line_weave()
if not line_ok then
    error(line_err, 0)
end
sleep(0)

local pose2_back_tcp = target_pose("pose_2_back", pose2_back)
show_target("pose_2_back", pose2_back_tcp, pose2_back)
moveJointWithAxisGroup(pose2_back.joint, joint_a, joint_v, 0, 0, group,
                       pose2_back.axis_q)
sleep(0)

local back_pose = target_pose("back", back)
show_target("back", back_pose, back)
moveJointWithAxisGroup(back.joint, joint_a, joint_v, 0, 0, group,
                       back.axis_q)
sleep(0)

show_target("origin", origin_pose, origin)
moveJointWithAxisGroup(origin.joint, joint_a, joint_v, 0, 0, group,
                       origin.axis_q)
sleep(0)