local M = {}

-- 导入依赖模块
local math = require("aubo.math")
local aubo = require("aubo")
local sched = sched or aubo.sched
local realtime = aubo.realtime or realtime
local sleep = sched.sleep
local poseTrans = math.poseTrans

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

local welding_common = require("welding_common")
local welding_laser = require("welding_laser")
local welding_motion = require("welding_motion")
local welding_communication = require("welding_communication")

-- 转换目标位姿到机器人基坐标系
function M.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

-- 等待条件满足
function M.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 welding_laser.getTargetPosInDeviceValidity() then
			return true -- 条件满足
		end
		-- 避免CPU占用过高
		sleep(sleep_time)
	end

	return false -- 超时未满足
end

-- 计算目标位姿并添加到action的laser_point_ids中
function M.calculateAndAddTargetPose(action)
	local _ENV = sched.select_robot(1)

	local function isLineLaserConfigError(err)
		if not err then
			return false
		end

		local err_str = tostring(err)
		return string.find(err_str, "laser_device_config", 1, true) ~= nil
			or string.find(err_str, "laser_offset", 1, true) ~= nil
	end

	-- 1. 获取目标在激光坐标系中的位置
	local target_in_laser, target_err = welding_laser.getTargetPosInDevice()
	if not target_in_laser then
		log_error(welding_common.log_prefix .. "Failed to get target pose in line laser: " .. tostring(target_err))
		if isLineLaserConfigError(target_err) then
			welding_common.showErrorMessage(welding_common.ERROR_ENUM.LINE_LASER_CONFIG_INVALID)
		else
			welding_common.showErrorMessage(welding_common.ERROR_ENUM.LINE_LASER_TARGET_POSE_READ_FAILED)
		end
		abort()
	end

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

	-- 3. 获取手眼标定向量
	local laser_device_config = welding_laser.getLaserDeviceConfig()
	if not laser_device_config or type(laser_device_config.hand_eye_calibration_results) ~= "table" then
		log_error(
			welding_common.log_prefix .. "Invalid line laser device config: hand_eye_calibration_results is missing"
		)
		welding_common.showErrorMessage(welding_common.ERROR_ENUM.LINE_LASER_CONFIG_INVALID)
		abort()
	end
	local hand_eye_vector = laser_device_config.hand_eye_calibration_results

	-- 4. 计算目标在机器人基坐标系中的位姿
	local result_tcp_pose = M.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
		welding_laser.addLaserData(action.laser_point_ids[1][2], result_tcp_pose, getJointPositions())
	else
		-- 错误处理: laser_point_ids结构不符合预期
		log_error(
			welding_common.log_prefix .. "Invalid action.laser_point_ids structure: cannot find UUID for data addition"
		)
	end
end

-- 创建直线圆弧路径段
function M.createSegment(line, start, via, end_point, start_rownum, end_rownum, start_j, via_j, end_j)
	return {
		["end"] = end_point,
		["end_j"] = end_j,
		["end_rownum"] = end_rownum,
		["line"] = line,
		["start"] = start,
		["start_j"] = start_j,
		["start_rownum"] = start_rownum,
		["via"] = via,
		["via_j"] = via_j,
	}
end

-- 创建参考坐标系数据
function M.createFrameData(h_plane, o, p)
	return { h_plane = h_plane, o = o, p = p }
end

-- 创建偏移值
function M.createOffset(y, z, a, b)
	return { a = a, b = b, x = 0.0, y = y, z = z }
end

-- 多层多道路径规划
function M.multiPath(h_plane, src_segs, mp_type, auto_frame, blender, offset, auto_optimize, point_o, point_p)
	local _ENV = sched.select_robot(1)
	-- 设置多道类型
	welding_communication.getFunctionResult("multipass.setWeldType", { mp_type })
	if auto_frame then
		-- 尝试自动设置参考坐标系
		local ret = welding_communication.getFunctionResult("multipass.trySetRefFrame", { src_segs, true })
		if ret ~= 0 then
			log_error(welding_common.log_prefix .. "TrySetRefFrame failed")
			welding_common.showErrorMessage(welding_common.ERROR_ENUM.OP_POINT_SETUP_FAILED)
		end
		assert(ret == 0, "Automatic setting of op point failed")
	else
		-- 或者 手动设置参考坐标系
		local frame = M.createFrameData(h_plane, point_o, point_p)
		local ret = welding_communication.getFunctionResult("multipass.setRefFrame", { frame })
		log_info(
			welding_common.log_prefix
				.. "setRefFrame()  "
				.. "h_plane:"
				.. tostring(h_plane)
				.. "  point_o:"
				.. welding_motion.formatPoseString(point_p)
				.. " point_p:"
				.. welding_motion.formatPoseString(point_p)
				.. " ret:"
				.. ret
		)
		if ret ~= 0 then
			log_error(welding_common.log_prefix .. "SetRefFrame failed")
			welding_common.showErrorMessage(welding_common.ERROR_ENUM.REFERENCE_FRAME_FAILED)
		end
		assert(ret == 0, "setRefFrame failed")
	end

	local segs_output = welding_communication.getFunctionResult(
		"multipass.getMultiplePaths",
		{ blender, auto_optimize, src_segs, offset }
	)
	return segs_output
end

-- 创建坐标偏移
function M.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

return M
