local M = {}

local json = require("dkjson")
local sched = sched or require("aubo").sched
local welding_common = require("welding_common")
local welding_motion = require("welding_motion")
local welding_arc = require("welding_arc")
local welding_config = require("welding_config")
local welding_laser = require("welding_laser")
local welding_utils = require("welding_utils")
local welding_math = require("welding_math")

-- 运行时快照：用于恢复时重新下发焊接参数
local last_welder_param_snapshot_ = nil
-- 恢复状态机：
-- 0: 正常运行
-- 1: 恢复流程中（未到恢复点）
-- 2: 已到恢复点，等待执行一次恢复动作
local resume_run_state_ = 0
local current_program_state_ = nil
local resume_target_point_index_ = -1
local resume_restore_context_ = {}
local resume_sync_done_ = false
local resume_just_restored_ = false
-- 仅用于“中断点在 MoveC 上”的一次性续跑缓存：
-- via 取回退计算结果 points[2]，end 取 stop_motion_next.poses[2]。
-- 由 initResumeContext 赋值，由 consumeResumeArcContinuation 单次消费并清空。
local resume_arc_via_pose_ = nil
local resume_arc_end_pose_ = nil
-- 回退距离非法时，设置该标志，防止 stop/pause 回调覆盖 last_runtime.json
local skip_runtime_cache_persist_ = false

-- last_runtime.json 文件路径
local function getRuntimeCachePath()
	return welding_common.arcs_ws_path .. "/config/Welding/last_runtime.json"
end

local function ensureRuntimeCacheDir()
	local runtime_cache_path = getRuntimeCachePath()
	local dir = runtime_cache_path:match("(.+)/[^/]+$")
	if dir and dir ~= "" then
		local ok = os.execute('mkdir -p "' .. dir .. '"')
		if ok == true or ok == 0 then
			log_info(welding_common.log_prefix .. "ensure resume cache dir success: " .. tostring(dir))
		else
			log_error(welding_common.log_prefix .. "ensure resume cache dir failed: " .. tostring(dir))
		end
	end
end

local function writeRuntimeCache(ctx)
	ensureRuntimeCacheDir()
	local cache_path = getRuntimeCachePath()
	local f = io.open(cache_path, "w")
	if not f then
		log_error(welding_common.log_prefix .. "write resume cache failed, open file failed: " .. tostring(cache_path))
		return false
	end
	local payload = json.encode(ctx or {})
	if not payload then
		log_error(welding_common.log_prefix .. "write resume cache failed, json encode returned nil")
		f:close()
		return false
	end
	f:write(payload)
	f:close()
	log_info(welding_common.log_prefix .. "write resume cache success: " .. tostring(cache_path))
	return true
end

local function readRuntimeCache()
	local cache_path = getRuntimeCachePath()
	local f = io.open(cache_path, "r")
	if not f then
		log_info(welding_common.log_prefix .. "resume cache not found: " .. tostring(cache_path))
		return nil, "cache file not found"
	end
	local content = f:read("*a")
	f:close()
	local obj, err = json.decode(content or "")
	if not obj then
		log_error(welding_common.log_prefix .. "decode resume cache failed: " .. tostring(err))
		return nil, err
	end
	log_info(welding_common.log_prefix .. "read resume cache success: " .. tostring(cache_path))
	return obj, nil
end

local function clearRuntimeCache()
	log_info(welding_common.log_prefix .. "clear resume cache")
	writeRuntimeCache({})
end

local function ensureRuntimeCacheFileExists()
	ensureRuntimeCacheDir()
	local cache_path = getRuntimeCachePath()
	local f = io.open(cache_path, "r")
	if f then
		f:close()
		log_info(welding_common.log_prefix .. "resume cache file already exists: " .. tostring(cache_path))
		return true
	end
	log_info(welding_common.log_prefix .. "resume cache file missing, create empty file: " .. tostring(cache_path))
	return writeRuntimeCache({})
end

local function setResumeRunState(state)
	local old_state = resume_run_state_
	resume_run_state_ = state or 0
	if old_state ~= resume_run_state_ then
		log_info(
			welding_common.log_prefix
				.. "resume_run_state changed: "
				.. tostring(old_state)
				.. " -> "
				.. tostring(resume_run_state_)
		)
	end
end

local function getResumeRunState()
	return resume_run_state_ or 0
end

-- 记录当前程序运行模式（来自上层状态：raw_path_weld/raw_path_sim/raw_point_sim）
-- 该状态会被恢复状态机读取，用于决定是否执行恢复动作（如恢复起弧）。
local function setCurrentProgramState(state)
	if type(state) == "string" and state ~= "" then
		current_program_state_ = state
	end
end

local function getCurrentProgramState()
	return current_program_state_
end

local function setResumeRestoreContext(ctx)
	resume_restore_context_ = ctx or {}
	resume_just_restored_ = false
	log_info(
		welding_common.log_prefix
			.. "set resume restore context, stopped_during_arcing="
			.. tostring(resume_restore_context_.stopped_during_arcing)
	)
end

local function getResumeRestoreContext()
	return resume_restore_context_ or {}
end

local function initResumeRunState(target_point_index, rollback_distance)
	resume_target_point_index_ = tonumber(target_point_index)
	local rollback = tonumber(rollback_distance)
	resume_sync_done_ = false
	if rollback and rollback >= 0 and resume_target_point_index_ > 0 then
		setResumeRunState(1)
		log_info(
			welding_common.log_prefix
				.. "init resume run state, cached target point index: "
				.. tostring(resume_target_point_index_)
				.. ", rollback_distance: "
				.. tostring(rollback)
		)
	else
		resume_target_point_index_ = -1
		setResumeRunState(0)
		log_info(welding_common.log_prefix .. "resume disabled by rollback or target point")
	end
end

local function onPointIndexUpdated(current_point_index)
	if resume_run_state_ == 1 and resume_target_point_index_ >= 0 then
		log_info(
			welding_common.log_prefix
				.. "resume tracking point, current="
				.. tostring(current_point_index)
				.. ", target="
				.. tostring(resume_target_point_index_)
		)
		if not resume_sync_done_ and (current_point_index or 0) == resume_target_point_index_ then
			welding_arc.syncProgramPoint()
			resume_sync_done_ = true
			log_info(
				welding_common.log_prefix .. "resume target reached, point index: " .. tostring(current_point_index)
			)
			setResumeRunState(2)
		end
	end
	return resume_run_state_
end

local function buildRuntimeContext()
	local _ENV = sched.select_robot(1)
	local stop_point = { move_mode = 0, joint = {}, pose = {} }
	local pose = getTcpPose()
	local q = getJointPositions()
	stop_point.pose = pose
	stop_point.joint = q

	local label = welding_utils.getAuboLabelComment and welding_utils.getAuboLabelComment() or nil
	if type(label) ~= "table" or label.point_index == nil then
		local welding = require("welding")
		local cached_label = welding.getCachedAuboLabelComment and welding.getCachedAuboLabelComment() or nil
		if type(cached_label) == "table" and cached_label.point_index ~= nil then
			label = cached_label
			log_info(
				welding_common.log_prefix
					.. "buildRuntimeContext fallback to cached label, point_index="
					.. tostring(label.point_index)
			)
		else
			log_warn(
				welding_common.log_prefix .. "buildRuntimeContext label unavailable, realtime and cached both empty"
			)
		end
	end
	local stop_idx = label and tonumber(label.point_index) or nil
	local stop_motion_prev = nil
	local stop_motion_next = nil
	if stop_idx ~= nil and welding_motion.getMotionQueue then
		local motion_queue = welding_motion.getMotionQueue()
		if type(motion_queue) == "table" then
			local target_idx = stop_idx + 1
			for _, entry in ipairs(motion_queue) do
				if entry and entry.point_index == stop_idx then
					stop_motion_prev = entry
				elseif entry and entry.point_index == (stop_idx + 1) then
					stop_motion_next = entry
				end
				if entry and entry.point_index == target_idx then
					stop_point.move_mode = entry.motion_type or 0
				end
				if stop_motion_prev and stop_motion_next then
					break
				end
			end
		end
	end

	local arc_stop_failed = welding_arc.isLastArcStopFailed and welding_arc.isLastArcStopFailed() or false
	local stopped_during_arcing = welding_arc.isArcing and welding_arc.isArcing() or false
	local arc_lost_detected = welding_config.isArcLostDetected and welding_config.isArcLostDetected() or false
	if arc_lost_detected then
		stopped_during_arcing = true
		log_info(welding_common.log_prefix .. "arc lost detected, force stopped_during_arcing=true for resume")
	end
	if arc_stop_failed then
		stopped_during_arcing = false
		log_info(
			welding_common.log_prefix
				.. "arc stop failed detected, force stopped_during_arcing=false to avoid re-arc on resume"
		)
	end
	log_info(
		welding_common.log_prefix
			.. "build runtime context, stop_idx="
			.. tostring(stop_idx)
			.. ", stop_move_mode="
			.. tostring(stop_point.move_mode)
			.. ", has_prev="
			.. tostring(stop_motion_prev ~= nil)
			.. ", has_next="
			.. tostring(stop_motion_next ~= nil)
			.. ", stopped_during_arcing="
			.. tostring(stopped_during_arcing)
	)

	return {
		last_reached_point_index = stop_idx,
		stop_point = stop_point,
		stop_motion_prev = stop_motion_prev,
		stop_motion_next = stop_motion_next,
		stopped_during_arcing = stopped_during_arcing,
		welder_param_snapshot = last_welder_param_snapshot_ or {},
		laser_data_store = welding_laser.getLaserData and welding_laser.getLaserData() or {},
	}
end

local function getEntryStartPose(entry, fallback_pose)
	if type(entry) ~= "table" or type(entry.poses) ~= "table" then
		return fallback_pose
	end
	if entry.motion_type == welding_motion.MoveMode.MoveC then
		return fallback_pose
	end
	return entry.poses[1] or fallback_pose
end

local function getEntryEndPose(entry, fallback_pose)
	if type(entry) ~= "table" or type(entry.poses) ~= "table" then
		return fallback_pose
	end
	if entry.motion_type == welding_motion.MoveMode.MoveC then
		return entry.poses[2] or fallback_pose
	end
	return entry.poses[1] or fallback_pose
end

local function calculateRollbackPath(cache, rollback_distance)
	if type(cache) ~= "table" or type(cache.stop_point) ~= "table" then
		return nil
	end
	local stop_pose = cache.stop_point.pose
	local prev = cache.stop_motion_prev
	local next = cache.stop_motion_next
	if type(stop_pose) ~= "table" or #stop_pose < 6 or type(next) ~= "table" then
		return nil
	end

	local start_pose = getEntryEndPose(prev, stop_pose)
	local ok = false
	local points = {}
	local calc_err = nil
	local is_arc = false
	log_info(
		welding_common.log_prefix
			.. "calculate rollback path, rollback_distance="
			.. tostring(rollback_distance)
			.. ", next_motion_type="
			.. tostring(next and next.motion_type)
	)

	if next.motion_type == welding_motion.MoveMode.MoveC and type(next.poses) == "table" then
		local mid_pose = next.poses[1]
		local end_pose = next.poses[2]
		if type(mid_pose) == "table" and type(end_pose) == "table" then
			is_arc = true
			points = { stop_pose, start_pose, mid_pose, end_pose }
			ok, points, calc_err = welding_math.getRollbackPoints(points, rollback_distance, 2)
		end
	else
		local end_pose = getEntryEndPose(next, nil)
		if type(end_pose) == "table" then
			points = { stop_pose, start_pose, end_pose }
			ok, points, calc_err = welding_math.getRollbackPoints(points, rollback_distance, 1)
		end
	end

	if ok and type(points) == "table" and #points > 0 and type(points[1]) == "table" and #points[1] >= 6 then
		if is_arc and #points >= 2 and type(points[2]) == "table" and #points[2] >= 6 then
			return { motion_type = welding_motion.MoveMode.MoveC, points = points }, nil
		end
		return { motion_type = welding_motion.MoveMode.MoveL, points = { points[1] } }, nil
	end
	log_error(welding_common.log_prefix .. "calculate rollback path failed, reason=" .. tostring(calc_err))
	return nil, calc_err
end

local function abortByRollbackDistanceExceeded()
	skip_runtime_cache_persist_ = true
	log_error(welding_common.log_prefix .. "rollback distance exceeds traveled path, abort resume")
	welding_common.showErrorMessage(welding_common.ERROR_ENUM.ROLLBACK_DISTANCE_EXCEEDED)
	abort()
end

-- 初始化断点续焊上下文。
-- rollback_distance/shared_params 用于回退路径计算与回退运动执行；
-- state 用于记录当前程序运行模式，供后续恢复状态机统一使用。
local function initResumeContext(rollback_distance, shared_params, state)
	setCurrentProgramState(state)
	log_info(welding_common.log_prefix .. "init resume context, rollback_distance: " .. tostring(rollback_distance))
	resume_arc_via_pose_ = nil
	resume_arc_end_pose_ = nil
	ensureRuntimeCacheFileExists()
	setResumeRestoreContext({})
	-- 如果没有回退距离或者小于 0 那么不执行回退继续
	if tonumber(rollback_distance) == nil or tonumber(rollback_distance) < 0 then
		setResumeRunState(0)
		log_info(welding_common.log_prefix .. "normal run, rollback disabled")
		return
	end

	-- 读取上一次运行缓存
	local cache, cache_err = readRuntimeCache()
	if cache and type(cache) == "table" and cache.last_reached_point_index ~= nil and cache.stop_point ~= nil then
		local need_resume_arcing = cache.stopped_during_arcing == true
		local rollback_path = nil
		log_info(
			welding_common.log_prefix
				.. "resume cache summary, need_resume_arcing="
				.. tostring(need_resume_arcing)
				.. ", last_reached_point_index="
				.. tostring(cache.last_reached_point_index)
		)
		initResumeRunState(cache.last_reached_point_index, rollback_distance)
		-- 计算回退点
		local rollback_err = nil
		rollback_path, rollback_err = calculateRollbackPath(cache, tonumber(rollback_distance) or 0)
		if rollback_err == "rollback_distance_exceeded" then
			abortByRollbackDistanceExceeded()
			return
		end
		if rollback_path then
			local rollback_tool_acc = tonumber(shared_params and shared_params.tool_acc) or 1.2
			local rollback_tool_speed = tonumber(shared_params and shared_params.tool_speed) or 0.05
			local rollback_blend_radius = tonumber(shared_params and shared_params.blend_radius) or 0.0
			local rollback_params = {
				motion_parameters = {
					tool_acc = rollback_tool_acc,
					tool_speed = rollback_tool_speed,
				},
				blend_radius = rollback_blend_radius,
			}

			-- 移动到抬高距离点
			local lift = tonumber(shared_params and shared_params.offset_distance) or 0
			if lift > 0 then
				local _ENV = sched.select_robot(1)
				local cur_pose = getTcpPose()
				local lifted_pose = poseTrans(cur_pose, { 0, 0, -lift, 0, 0, 0 })
				log_info(
					welding_common.log_prefix
						.. "resume pre-lift moveLine: "
						.. welding_motion.formatPoseString(lifted_pose)
				)
				welding_motion.generateMoveLine(
					lifted_pose,
					rollback_params.motion_parameters.tool_speed,
					rollback_params,
					"raw_path_sim",
					false,
					0,
					false,
					true
				)
				sched.sync_program_point()
			end

			-- 移动到回退点
			local rollback_pose = rollback_path.points[1]
			log_info(
				welding_common.log_prefix .. "rollback by line: " .. welding_motion.formatPoseString(rollback_pose)
			)
			welding_motion.generateMoveLine(
				rollback_pose,
				rollback_params.motion_parameters.tool_speed,
				rollback_params,
				"raw_path_sim",
				false,
				0,
				false,
				true
			)
			sched.sync_program_point()
			log_info(welding_common.log_prefix .. "rollback motion finished and synced")
		else
			log_info(welding_common.log_prefix .. "rollback pose calculation skipped or failed")
		end
		resume_arc_via_pose_ = nil
		resume_arc_end_pose_ = nil
		if
			rollback_path
			and rollback_path.motion_type == welding_motion.MoveMode.MoveC
			and rollback_path.points
			and #rollback_path.points >= 2
		then
			resume_arc_via_pose_ = rollback_path.points[2]
			if cache.stop_motion_next and type(cache.stop_motion_next.poses) == "table" then
				resume_arc_end_pose_ = cache.stop_motion_next.poses[2]
			end
		end

		setResumeRestoreContext({
			welder_param_snapshot = cache.welder_param_snapshot or {},
			stopped_during_arcing = need_resume_arcing,
		})
		if cache.laser_data_store and welding_laser.resetLaserData then
			welding_laser.resetLaserData(cache.laser_data_store)
			log_info(welding_common.log_prefix .. "laser data restored from resume cache")
		end
		log_info(
			welding_common.log_prefix
				.. "resume context loaded, last_reached_point_index: "
				.. tostring(cache.last_reached_point_index)
		)
	else
		log_info(
			welding_common.log_prefix .. "resume cache unavailable, fallback to normal run: " .. tostring(cache_err)
		)
		setResumeRunState(0)
		setResumeRestoreContext({})
	end
end

local function recordWelderParamSnapshot(welder_param)
	if type(welder_param) == "table" then
		last_welder_param_snapshot_ = welding_utils.deepCopy(welder_param)
		log_info(welding_common.log_prefix .. "record welder param snapshot")
	end
end

local function applyRestoreIfNeeded(state)
	local resume_run_state = getResumeRunState()
	if resume_run_state ~= 2 then
		return resume_run_state
	end

	local restore_ctx = getResumeRestoreContext()
	log_info(welding_common.log_prefix .. "apply resume restore action, state=" .. tostring(state))
	if
		restore_ctx.stopped_during_arcing
		and type(restore_ctx.welder_param_snapshot) == "table"
		and state == "raw_path_weld"
	then
		local p = restore_ctx.welder_param_snapshot
		welding_config.updateWeldingParameters(
			p.current or 0,
			p.voltage or 0,
			p.stop_arc_voltage or 0,
			p.stop_arc_current or 0,
			p.stop_arc_wait_time or 0,
			p.delivery_time or 0,
			p.arc_voltage or 0,
			p.arc_current or 0,
			p.start_arc_wait_time or 0
		)
		log_info(welding_common.log_prefix .. "restore welding parameters finished")
	end

	if restore_ctx.stopped_during_arcing and state == "raw_path_weld" then
		log_info(welding_common.log_prefix .. "restore arc state: start arc")
		welding_arc.startArc()
	end

	-- 标记“刚完成恢复动作”：
	-- 该标记用于后续首次摆焊/圆弧续跑时，将起点切到当前 TCP（避免回到原轨迹首点）。
	resume_just_restored_ = true
	setResumeRunState(0)
	log_info(welding_common.log_prefix .. "resume restore done, switch to normal run state")
	return 0
end

-- 单次消费“刚恢复完成”标记。
-- 返回 true: 本次调用后，调用方可执行一次恢复后特殊分支（如以当前 TCP 作为续跑起点）。
-- 返回 false: 无恢复后特殊分支，按正常流程执行。
-- 注意：该函数为一次性语义，读取后会立即清零标记。
local function consumeResumeJustRestoredFlag()
	local v = resume_just_restored_
	resume_just_restored_ = false
	return v
end

-- 单次消费圆弧续跑参数（via/end）。
-- 当停止点处于 MoveC 且可计算续跑圆弧时，initResumeContext 会预置 via/end。
-- 调用方在“第一段需要续圆弧”的位置消费本函数返回值：
-- 1) 返回 table { via_pose=..., end_pose=... }：应用该圆弧继续未完成段；
-- 2) 返回 nil：无可用续圆弧参数，按原始轨迹点执行。
-- 注意：该函数为一次性语义，成功返回后内部缓存会被清空，避免重复续跑。
local function consumeResumeArcContinuation()
	if
		type(resume_arc_via_pose_) == "table"
		and #resume_arc_via_pose_ >= 6
		and type(resume_arc_end_pose_) == "table"
		and #resume_arc_end_pose_ >= 6
	then
		local via = resume_arc_via_pose_
		local ending = resume_arc_end_pose_
		resume_arc_via_pose_ = nil
		resume_arc_end_pose_ = nil
		return {
			via_pose = via,
			end_pose = ending,
		}
	end
	return nil
end

-- state=1 时副作用指令过滤：
-- 允许流程推进、标签/上下文更新、路径计算；
-- 禁止真实运动外设动作（起弧/熄弧/参数下发/摆焊/激光采集等）。
local function shouldSkipInstructionInState1(inst_type)
	if
		inst_type == "Welder_ArcOn"
		or inst_type == "Welder_ArcOff"
		or inst_type == "Welder_UpdateParameter"
		or inst_type == "Welder_ArcOffPra"
		or inst_type == "Robot_WeaveStart"
		or inst_type == "Robot_WeaveEnd"
		or inst_type == "Robot_MoveWeave"
		or inst_type == "LineLaser_Open"
		or inst_type == "LineLaser_Close"
		or inst_type == "Collect_Start"
		or inst_type == "Collect_Stop"
		or inst_type == "Track_Start"
		or inst_type == "Track_Stop"
	then
		return true
	end
	return false
end

-- 统一恢复态解析入口（运行模式由 setCurrentProgramState/initResumeContext 维护）：
-- 1) 推进恢复状态机（命中恢复点时触发 applyRestoreIfNeeded）；
-- 2) 计算当前是否执行真实运动 execute_motion；
-- 3) 若传入 inst_type，额外给出 state=1 下的指令级过滤结果。
-- 返回值: resume_run_state, execute_motion, should_skip_instruction
-- 调用约定：
-- - 推荐在每个“可能发运动/副作用指令”的处理点实时调用，避免只在函数入口取一次状态。
-- - 当返回 execute_motion=false 时，调用方仍可执行标签推进与路径计数，但应禁用真实外设动作。
local function resolveResumeExecutionState(inst_type)
	local state = current_program_state_
	local resume_run_state = tonumber(getResumeRunState and getResumeRunState() or 0) or 0
	local execute_motion = (resume_run_state ~= 1)

	resume_run_state = tonumber(applyRestoreIfNeeded and applyRestoreIfNeeded(state) or resume_run_state)
		or resume_run_state
	execute_motion = (resume_run_state ~= 1)

	local should_skip_instruction = false
	if inst_type ~= nil and resume_run_state == 1 and shouldSkipInstructionInState1(inst_type) then
		should_skip_instruction = true
	end

	return resume_run_state, execute_motion, should_skip_instruction
end

local function onProgramStop(is_normal)
	log_info(welding_common.log_prefix .. "onProgramStop, normal=" .. tostring(is_normal))
	if skip_runtime_cache_persist_ then
		log_info(welding_common.log_prefix .. "skip runtime cache persist for rollback distance exceeded")
		skip_runtime_cache_persist_ = false
		return
	end
	if not is_normal then
		writeRuntimeCache(buildRuntimeContext())
	else
		clearRuntimeCache()
	end
end

local function onProgramPause()
	log_info(welding_common.log_prefix .. "onProgramPause, persist resume cache")
	if skip_runtime_cache_persist_ then
		log_info(welding_common.log_prefix .. "skip pause cache persist for rollback distance exceeded")
		return
	end
	writeRuntimeCache(buildRuntimeContext())
end

local function setupMotionHook()
	if welding_motion.setPointIndexUpdateCallback then
		welding_motion.setPointIndexUpdateCallback(onPointIndexUpdated)
		log_info(welding_common.log_prefix .. "resume motion callback registered")
	end
end

setupMotionHook()

M.getRuntimeCachePath = getRuntimeCachePath
M.writeRuntimeCache = writeRuntimeCache
M.readRuntimeCache = readRuntimeCache
M.clearRuntimeCache = clearRuntimeCache
M.initResumeContext = initResumeContext
M.recordWelderParamSnapshot = recordWelderParamSnapshot
M.applyRestoreIfNeeded = applyRestoreIfNeeded
M.shouldSkipInstructionInState1 = shouldSkipInstructionInState1
M.resolveResumeExecutionState = resolveResumeExecutionState
M.onProgramStop = onProgramStop
M.onProgramPause = onProgramPause
M.getResumeRunState = getResumeRunState
M.setCurrentProgramState = setCurrentProgramState
M.getCurrentProgramState = getCurrentProgramState
M.setResumeRunState = setResumeRunState
M.onPointIndexUpdated = onPointIndexUpdated
M.consumeResumeJustRestoredFlag = consumeResumeJustRestoredFlag
M.consumeResumeArcContinuation = consumeResumeArcContinuation

return M
