-- 纯外部轴脚本，不包含示教器生成的程序结构。
-- 外部轴方向基于机器人基坐标系：第一轴为{0,0,1}，第二轴为{0,1,0}，第三轴为{1,0,0}。

local group = "轴组_1"

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

local fake_axis2 = app.api:getAxisInterface("fake_axis2")
fake_axis2:setExtAxisType(1)
fake_axis2:setExtAxisMaxPositionLimit(1000)
fake_axis2:setExtMinPositionLimit(0)
fake_axis2:setExtAxisMaxVelocityLimit(1000)
fake_axis2:setExtAxisMaxAccelerationLimit(1000)
fake_axis2:setReductionRatio(1)

local fake_axis3 = app.api:getAxisInterface("fake_axis3")
fake_axis3:setExtAxisType(1)
fake_axis3:setExtAxisMaxPositionLimit(4000)
fake_axis3:setExtMinPositionLimit(0)
fake_axis3:setExtAxisMaxVelocityLimit(1000)
fake_axis3:setExtAxisMaxAccelerationLimit(1000)
fake_axis3:setReductionRatio(1)

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

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 require_target(name, target)
    if type(target) ~= "table" then
        error(name .. " invalid, expected target table", 2)
    end
    local joint = target.joint
    local axis_xyz = target.axis_xyz or target.axis_q
    if joint == nil then
        require_vector(name, target, 9)
        joint = {target[1], target[2], target[3], target[4], target[5], target[6]}
        axis_xyz = {target[7], target[8], target[9]}
    else
        require_vector(name .. ".joint", joint, 6)
        require_vector(name .. ".axis_xyz", axis_xyz, 3)
    end
    return {
        joint = joint_deg_to_rad(joint),
        axis_q = {mm_to_m(axis_xyz[3]), mm_to_m(axis_xyz[2]),
                  mm_to_m(axis_xyz[1])}
    }
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关节，后3个为BASE的X/Y/Z外部轴mm，内部转m。

local origin = require_target("origin_target", origin_target)
local target1 = require_target("line_target1", line_target1)
local target2 = require_target("line_target2", line_target2)
local return_target = require_target("return_target", 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 = target_pose("line1", target1)
show_target("line1", pose1, target1)
moveJointWithAxisGroup(target1.joint, joint_a, joint_v, 0, 0, group,
                       target1.axis_q)
sleep(0)

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

local return_pose = target_pose("return", return_target)
show_target("return", return_pose, return_target)
moveJointWithAxisGroup(return_target.joint, joint_a, joint_v, 0, 0, group,
                       return_target.axis_q)
sleep(0)

show_target("back origin axis", origin_pose, {
    joint = return_target.joint,
    axis_q = origin.axis_q
})
moveJointWithAxisGroup(return_target.joint, joint_a, joint_v, 0, 0, group,
                       origin.axis_q)
sleep(0)