local M = {}

local sched = sched or require("aubo").sched
local math = math or require("aubo").math

local welding_common = require("welding_common")

-- ==================== 工具函数 ====================

-- 判断两个点位是否相似（前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 normalizeLabelComment(str)
	if not str or str == "" then
		return "WELDING:comment=;point_index="
	end

	-- 已经是规范格式
	if string.sub(str, 1, 8) == "WELDING:" then
		return str
	end

	-- 普通字符串 -> 转成规范格式
	return string.format("WELDING:comment=%s;point_index=", tostring(str))
end

local function parseLabelComment(str)
	local result = {}

	if not str or str == "" then
		return result
	end

	str = string.gsub(str, "^WELDING:", "")

	for k, v in string.gmatch(str, "([^=;]+)=([^;]*)") do
		result[k] = v
	end

	return result
end

local function updateAuboLabel(new_fields)
	local _ENV = sched.select_robot(1)

	-- 规定主协程进行更新标签，所以传入 getMainThreadId()
	local _, line_num, comment = getAdvancePlanContext(welding_common.getMainThreadId())
	log_info(welding_common.log_prefix .. "getAdvancePlanContext: ~line_num: " .. line_num .. " ~comment: " .. comment)

	comment = normalizeLabelComment(comment)

	local old_data = parseLabelComment(comment)

	-- 只覆盖指定字段
	for k, v in pairs(new_fields) do
		old_data[k] = v
	end

	local str = string.format(
		"WELDING:comment=%s;point_index=%s",
		tostring(old_data.comment or ""),
		tostring(old_data.point_index or "")
	)

	log_info(
		welding_common.log_prefix .. "setWeldingLabel(updateAuboLabel): ~line_num: " .. line_num .. " ~comment: " .. str
	)
	setLabel(line_num, str)

	return line_num
end

local function auboSetLabel(line_num)
	local _ENV = sched.select_robot(1)

	-- 规定主协程进行更新标签，所以传入 getMainThreadId()
	local _, _, comment = getAdvancePlanContext(welding_common.getMainThreadId())
	log_info(welding_common.log_prefix .. "getAdvancePlanContext ~comment: " .. comment)
	comment = normalizeLabelComment(comment)

	log_info(welding_common.log_prefix .. "setWeldingLabel: ~line_num" .. line_num .. " ~comment: " .. comment)
	setLabel(line_num, comment)
end

local function setAuboLabelPointIndex(index)
	local data = {}

	if index ~= nil then
		data.point_index = index
	end

	return updateAuboLabel(data)
end

local function getAuboLabelComment(print_out)
	local _ENV = sched.select_robot(1)
	if print_out == nil then
		print_out = true
	end

	-- 子协程执行命令的话当前 tid 会变成子协程，所以固定获取 getMainThreadId()
	local _, line_num, comment = getPlanContext(welding_common.getMainThreadId())
	if print_out then
		log_info(welding_common.log_prefix .. "getPlanContext: ~line_num: " .. line_num .. " ~comment: " .. comment)
	end

	comment = normalizeLabelComment(comment)
	local data = parseLabelComment(comment)

	if print_out then
		log_info(
			welding_common.log_prefix
				.. "getWeldingLabelComment: ~line_num: "
				.. line_num
				.. " ~comment: "
				.. data.comment
				.. " ~point_index: "
				.. data.point_index
		)
	end

	return {
		line_num = line_num,
		comment = data.comment or "",
		point_index = tonumber(data.point_index),
	}
end

-- 导出模块
M.arePointsSimilar = arePointsSimilar
M.deepCopy = deepCopy
M.auboSetLabel = auboSetLabel
M.setAuboLabelPointIndex = setAuboLabelPointIndex
M.getAuboLabelComment = getAuboLabelComment

return M
