local M = {}

-- 导入必要的模块
local sched = sched or require("aubo").sched
local math = math or require("aubo").math

-- 导入错误处理模块
local welding_common = require("welding_common")
-- 导入配置模块
local welding_config = require("welding_config")
local welding_utils = require("welding_utils")
local welding_arc = require("welding_arc")
local welding_math = require("welding_math")

-- 运动类型枚举，对应 C++ 定义的 MoveMode
local MoveMode = {
	None = 0,
	MoveJ = 1,
	MoveL = 2,
	MoveC = 3,
	MoveP = 4,
}

-- 起弧时的运动队列
local arcing_motion_queue = {}
local arcing_motion_queue_mutex = { locked = false }
-- 全部运动调用队列（记录所有运动调用）
local motion_queue_index = 0
local motion_queue = {}
local motion_queue_mutex = { locked = false }
local point_index_update_cb_ = nil

local function acquireMotionQueueLock()
	local _ENV = sched.select_robot(1)
	while true do
		if not motion_queue_mutex.locked then
			motion_queue_mutex.locked = true
			return
		end
		sched.sleep(0.001)
		sched.sync()
	end
end

local function releaseMotionQueueLock()
	motion_queue_mutex.locked = false
end

local function acquireArcingMotionQueueLock()
	local _ENV = sched.select_robot(1)
	while true do
		if not arcing_motion_queue_mutex.locked then
			arcing_motion_queue_mutex.locked = true
			return
		end
		sched.sleep(0.001)
		sched.sync()
	end
end

local function releaseArcingMotionQueueLock()
	arcing_motion_queue_mutex.locked = false
end

-- 统一记录每次 move 调用：写入 motion_queue；若当前处于起弧或分组统计阶段则同时写入 arcing_motion_queue
local function recordMotion(motion_type, point_repr, weld_type)
	local entry = nil

	local ok, err = pcall(function()
		acquireMotionQueueLock()
		acquireArcingMotionQueueLock()

		motion_queue_index = motion_queue_index + 1

		local poses
		if motion_type == MoveMode.MoveC then
			poses = point_repr
		else
			poses = { point_repr }
		end

		local distance = 0

		local should_track_arcing_queue = false
		if welding_arc then
			should_track_arcing_queue = welding_arc.isArcing()
				or (welding_arc.isGroupedRecording and welding_arc.isGroupedRecording())
		end

		if should_track_arcing_queue then
			local prev_entry = nil

			if #motion_queue >= 1 then
				prev_entry = motion_queue[#motion_queue]
			end

			if prev_entry and prev_entry.poses and poses then
				local prev_pose = prev_entry.poses[#prev_entry.poses]
				local curr_pose = poses[1]

				if motion_type == MoveMode.MoveC then
					distance = welding_math.arcLength(prev_pose, poses[1], poses[2])
				else
					distance = welding_math.distance(prev_pose, curr_pose)
				end
			end

			table.insert(arcing_motion_queue, {
				point_index = motion_queue_index,
			})
		end

		entry = {
			point_index = motion_queue_index,
			motion_type = motion_type,
			weld_type = weld_type,
			poses = poses,
			distance_to_prev = distance,
		}

		table.insert(motion_queue, entry)

		releaseArcingMotionQueueLock()
		releaseMotionQueueLock()
	end)

	-- 确保锁一定释放
	releaseArcingMotionQueueLock()
	releaseMotionQueueLock()

	if not ok then
		error(err)
	end

	return entry
end

local function getCurrentPointIndex()
	return motion_queue_index or 0
end

local function setPointIndexUpdateCallback(cb)
	point_index_update_cb_ = cb
end

local function notifyPointIndexUpdated()
	if type(point_index_update_cb_) == "function" then
		local ok, err = pcall(point_index_update_cb_, motion_queue_index or 0)
		if not ok then
			log_error(welding_common.log_prefix .. "point index callback failed: " .. tostring(err))
		end
	end
end

-- ==================== 运动指令生成器 ====================

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

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

local function inverseSolutionSuccess(point_q, point_tcp_pose)
	local _ENV = sched.select_robot(1)
	local q, err = inverseKinematics(point_q, point_tcp_pose)
	log_info(
		welding_common.log_prefix
			.. "inverseKinematics:"
			.. "q:"
			.. formatPoseString(point_q)
			.. "p:"
			.. formatPoseString(point_tcp_pose)
	)
	log_info(welding_common.log_prefix .. "inverseKinematics err:" .. err)
	if err ~= 0 then
		welding_common.showErrorMessage(welding_common.ERROR_ENUM.INVERSE_SOLUTION_FAILED)
	end

	return q, err
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_common.log_prefix .. "point_in_Workpiece" .. formatPoseString(point_in_Workpiece))
	return point_in_Workpiece
end

local function generateMoveJoint(point, params, weld_type, execute_motion)
	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 inv_q, err = inverseSolutionSuccess(formatPose(q), formatPose(tcp_pose))

	local print_out = string.format(
		"moveJoint(%s,%f,%f,%f,0)",
		formatPoseString(inv_q),
		params.motion_parameters.joint_acc or 80.0,
		params.motion_parameters.joint_speed or 60.0,
		0.0
	)
	log_info(welding_common.log_prefix .. print_out)

	if err ~= 0 then
		return
	end

	-- 记录本次运动到通用队列（统一存位姿）
	local pose_repr = formatPose(tcp_pose)
	recordMotion(MoveMode.MoveJ, pose_repr, weld_type)

	if execute_motion ~= false then
		moveJoint(
			inv_q,
			params.motion_parameters.joint_acc or 80.0,
			params.motion_parameters.joint_speed or 60.0,
			0.0,
			0
		)
	else
		log_info(welding_common.log_prefix .. "skip moveJoint in resume state, target=" .. formatPoseString(tcp_pose))
	end
	welding_utils.setAuboLabelPointIndex(motion_queue_index)

	notifyPointIndexUpdated()

	return print_out
end

local function generateMoveLine(
	point,
	speed,
	params,
	state,
	is_welding_endpoints,
	weld_type,
	should_record,
	execute_motion
)
	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_common.log_prefix .. "Pose data is empty or dimension mismatch (expected 6D)")
		welding_common.showErrorMessage(welding_common.ERROR_ENUM.POSE_DATA_EMPTY)
		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_common.log_prefix .. "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_common.log_prefix .. print_out)

	-- 记录本次运动到通用队列（统一存位姿）
	if should_record ~= false then
		recordMotion(MoveMode.MoveL, formatted_pose, weld_type)
	end

	if execute_motion ~= false then
		moveLine(formatted_pose, params.motion_parameters.tool_acc or 1.2, run_speed, blend_radius, 0)
	else
		log_info(
			welding_common.log_prefix .. "skip moveLine in resume state, target=" .. formatPoseString(formatted_pose)
		)
	end

	if should_record ~= false then
		welding_utils.setAuboLabelPointIndex(motion_queue_index)
	end

	notifyPointIndexUpdated()

	return print_out
end

local function generateMoveP(point, speed, params, state, is_welding_endpoints, weld_type, execute_motion)
	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_common.log_prefix .. "Pose data is empty or dimension mismatch (expected 6D)")
		welding_common.showErrorMessage(welding_common.ERROR_ENUM.POSE_DATA_EMPTY)
		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_common.log_prefix .. "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_common.log_prefix .. print_out)

	-- 记录本次运动到通用队列（统一存位姿）
	recordMotion(MoveMode.MoveP, formatted_pose, weld_type)

	if execute_motion ~= false then
		moveProcess(formatted_pose, params.motion_parameters.tool_acc or 1.2, run_speed, blend_radius)
	else
		log_info(
			welding_common.log_prefix .. "skip moveProcess in resume state, target=" .. formatPoseString(formatted_pose)
		)
	end
	welding_utils.setAuboLabelPointIndex(motion_queue_index)

	notifyPointIndexUpdated()

	return print_out
end

local function generateMoveArc(
	point1,
	point2,
	speed,
	params,
	state,
	is_welding_endpoints,
	weld_type,
	should_record,
	execute_motion
)
	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_common.log_prefix .. "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
	)
	if execute_motion ~= false then
		log_info(welding_common.log_prefix .. "setCirclePathMode(2)")
		setCirclePathMode(2)
	end
	log_info(welding_common.log_prefix .. print_out)
	-- 入队逻辑（记录弧终点作为点位）
	local end_pose = formatPose(point2_pose)

	-- 记录本次运动到通用队列（统一存位姿）
	if should_record ~= false then
		recordMotion(MoveMode.MoveC, { formatPose(point1_pose), end_pose }, weld_type)
	end

	if execute_motion ~= false then
		moveCircle(formatPose(point1_pose), end_pose, params.motion_parameters.tool_acc, run_speed, blend_radius, 0)
	else
		log_info(
			welding_common.log_prefix
				.. "skip moveCircle in resume state, via="
				.. formatPoseString(point1_pose)
				.. ", target="
				.. formatPoseString(end_pose)
		)
	end

	if should_record ~= false then
		welding_utils.setAuboLabelPointIndex(motion_queue_index)
	end

	notifyPointIndexUpdated()

	return print_out
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, welding_common.ErrorCode.WAYPOINT.POSE_DIMENSION_MISMATCH
	end
	if not q or #q ~= 6 then
		return false, welding_common.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 getArcingMotionQueue()
	acquireArcingMotionQueueLock()
	local copy = {}
	for i, v in ipairs(arcing_motion_queue) do
		copy[i] = v
	end
	releaseArcingMotionQueueLock()
	return copy
end

local function clearArcingMotionQueue()
	acquireArcingMotionQueueLock()
	arcing_motion_queue = {}
	releaseArcingMotionQueueLock()
end

local function getMotionQueue()
	acquireMotionQueueLock()
	local copy = {}
	for i, v in ipairs(motion_queue) do
		copy[i] = v
	end
	releaseMotionQueueLock()
	return copy
end

-- 导出模块
M.formatPose = formatPose
M.formatPoseString = formatPoseString
M.calculateOffsetPoint = calculateOffsetPoint
M.inverseSolutionSuccess = inverseSolutionSuccess
M.coordinateSystemTransformationOfPoints = coordinateSystemTransformationOfPoints
M.generateMoveJoint = generateMoveJoint
M.generateMoveLine = generateMoveLine
M.generateMoveP = generateMoveP
M.generateMoveArc = generateMoveArc
M.validatePoint = validatePoint
M.validateWeldingParams = validateWeldingParams
M.getArcingMotionQueue = getArcingMotionQueue
M.clearArcingMotionQueue = clearArcingMotionQueue
M.getMotionQueue = getMotionQueue
M.recordMotion = recordMotion
M.MoveMode = MoveMode
M.getCurrentPointIndex = getCurrentPointIndex
M.setPointIndexUpdateCallback = setPointIndexUpdateCallback

return M
