local M = {}

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

-- 导入错误处理模块
local welding_common = require("welding_common")
local welding_communication = require("welding_communication")

-- ==================== 线激光识别路点 ====================
-- 创建一个表来存储数据
-- 现在每个键对应的值是一个包含两组数据的表：{tcp_pose = {...}, joint_angles = {...}}
local laser_data_store_ = {}

-- 添加函数：向表中添加键和对应的值（包含tcp_pose和joint_angles两组数据）
-- 参数：key - 键名(如"a")，tcp_pose - TCP位姿数据列表(如{1,2,3,4,5,6})，joint_angles - 关节角数据列表(如{7,8,9,10,11,12})
local function addLaserData(key, tcp_pose, joint_angles)
	-- 检查输入参数是否有效
	if not tcp_pose or not joint_angles then
		return
	end

	-- 存储数据
	laser_data_store_[key] = {
		tcp_pose = tcp_pose,
		joint_angles = joint_angles,
	}
	log_info(
		welding_common.log_prefix
			.. "================AddLaserData: "
			.. key
			.. " : "
			.. tcp_pose[1]
			.. ","
			.. tcp_pose[2]
			.. ","
			.. tcp_pose[3]
			.. ","
			.. tcp_pose[4]
			.. ","
			.. tcp_pose[5]
			.. ","
			.. tcp_pose[6]
	)
end

-- 取用函数：从表中获取指定键的完整数据（包含tcp_pose和joint_angles两组数据）
local function getLaserData(key)
	if key == nil then
		return laser_data_store_ or {}
	end
	if type(key) ~= "string" then
		return nil
	end
	local stored_data = laser_data_store_[key]
	log_info(welding_common.log_prefix .. "GetLaserData:" .. key)
	if stored_data then
		return stored_data
	end
	return nil
end

local function resetLaserData(store)
	if type(store) == "table" then
		laser_data_store_ = store
	else
		laser_data_store_ = {}
	end
end

--==================== 线激光传感器 ====================
local LASER_TRACK_RECORD_METHOD = "LineLaser.trackRecordSeekTcpPose"
local LASER_TRACK_OFFSET_METHOD = "LineLaser.trackExpectPosOffsetInLine"
local SET_HANDEYE_METHOD = "LineLaser.setHandEye"
local SET_LINE_LASER_TYPE_METHOD = "LineLaser.setLineLaserType"
local TRACKING_INIT_METHOD = "LineLaser.trackingInit"
local TRACKING_DEINIT_METHOD = "LineLaser.trackingDeinit"
local SET_FILTER_METHOD = "LineLaser.setFilter"
local SET_TIME_OFFSET_MS_METHOD = "LineLaser.setPoseMatchOffsetTime"
local SET_MAX_HISTORY_METHOD = "LineLaser.setLaserTcpCacheSize"
local SET_LASER_OFFSET_METHOD = "LineLaser.setLaserOffset"

local laser_device_config = nil

local function formatTableForLog(value)
	if value == nil then
		return "nil"
	end

	local ok, encoded = pcall(json.encode, value)
	if ok then
		return encoded
	end

	return tostring(value)
end

local function setHandEye(hand_eye_calib_result, hand_eye_calib_error)
	local result_list = hand_eye_calib_result or {}
	local calib_error = hand_eye_calib_error
	if calib_error == nil then
		calib_error = 0
	end

	log_info(
		welding_common.log_prefix
			.. "setHandEye input results="
			.. formatTableForLog(result_list)
			.. ", error="
			.. tostring(calib_error)
	)

	local result, err = welding_communication.getFunctionResult(SET_HANDEYE_METHOD, { result_list, calib_error })
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. SET_HANDEYE_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

local function setLineLaserType(linelaser_type)
	local result, err = welding_communication.getFunctionResult(SET_LINE_LASER_TYPE_METHOD, { linelaser_type })
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. SET_LINE_LASER_TYPE_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

local function trackingInit(points, max_difference_distance_for_key_search)
	local weld_key_points = points or {}
	local max_difference = max_difference_distance_for_key_search
	if max_difference == nil then
		max_difference = 0.003
	end

	log_info(
		welding_common.log_prefix
			.. "trackingInit points="
			.. formatTableForLog(weld_key_points)
			.. ", max_difference_distance_for_key_search="
			.. tostring(max_difference)
	)

	local result, err =
		welding_communication.getFunctionResult(TRACKING_INIT_METHOD, { weld_key_points, max_difference })
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. TRACKING_INIT_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

local function trackingDeinit()
	local result, err = welding_communication.getFunctionResult(TRACKING_DEINIT_METHOD, {})
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. TRACKING_DEINIT_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

local function setFilter(filter_type, filter_window_size)
	local result, err = welding_communication.getFunctionResult(SET_FILTER_METHOD, { filter_type, filter_window_size })
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. SET_FILTER_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

local function setPoseMatchOffsetTime(ms)
	local result, err = welding_communication.getFunctionResult(SET_TIME_OFFSET_MS_METHOD, { ms })
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. SET_TIME_OFFSET_MS_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

local function setLaserTcpCacheSize(size)
	local result, err = welding_communication.getFunctionResult(SET_MAX_HISTORY_METHOD, { size })
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. SET_MAX_HISTORY_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

local function setLaserDeviceConfig(config)
	-- 激光寻位
	laser_device_config = config
	if not config then
		log_info(welding_common.log_prefix .. "setLaserDeviceConfig received nil config")
		return
	end

	log_info(welding_common.log_prefix .. "setLaserDeviceConfig config=" .. formatTableForLog(config))

	local hand_eye_results = config.hand_eye_calibration_results or {}
	local hand_eye_error = config.hand_eye_calibration_error
	if hand_eye_error == nil then
		hand_eye_error = 0
	end

	log_info(
		welding_common.log_prefix
			.. "setLaserDeviceConfig hand_eye_calibration_results size="
			.. tostring(#hand_eye_results)
			.. ", values="
			.. formatTableForLog(hand_eye_results)
			.. ", hand_eye_calibration_error="
			.. tostring(hand_eye_error)
	)

	-- 激光跟踪设置
	setHandEye(hand_eye_results, hand_eye_error)

	-- 设置线激光偏移量到跟踪服务端
	if type(config.laser_offset) == "table" then
		local ox = tonumber(config.laser_offset.x) or 0
		local oy = tonumber(config.laser_offset.y) or 0
		local oz = tonumber(config.laser_offset.z) or 0

		log_info(
			welding_common.log_prefix
				.. "setLaserDeviceConfig laser_offset: x="
				.. tostring(ox)
				.. ", y="
				.. tostring(oy)
				.. ", z="
				.. tostring(oz)
		)
		local result, err = welding_communication.getFunctionResult(SET_LASER_OFFSET_METHOD, { ox, oy, oz })
		if err then
			log_error(
				welding_common.log_prefix .. "Failed to call " .. SET_LASER_OFFSET_METHOD .. ": " .. tostring(err)
			)
		end
	end
end

local function getLaserDeviceConfig()
	return laser_device_config
end

--[[ 记录当前寻位 TCP 位姿到跟踪服务端
    @return result 服务端返回值，失败时返回 nil
]]
local function trackRecordSeekTcpPose()
	local result, err = welding_communication.getFunctionResult(LASER_TRACK_RECORD_METHOD, {})
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. LASER_TRACK_RECORD_METHOD .. ": " .. tostring(err))
		return nil
	end
	return result
end

--[[ 获取当前直线跟踪期望位姿偏移
    @return table|nil 偏移位姿（期望为{x,y,z,rx,ry,rz}或等价数组），失败/无数据时返回nil
]]
local function trackExpectPosOffsetInLine()
	local offset, err = welding_communication.getFunctionResult(LASER_TRACK_OFFSET_METHOD, {})
	if err then
		log_error(welding_common.log_prefix .. "Failed to call " .. LASER_TRACK_OFFSET_METHOD .. ": " .. tostring(err))
		return nil
	end

	if offset ~= nil and type(offset) ~= "table" then
		log_error(
			welding_common.log_prefix
				.. "Invalid offset type from "
				.. LASER_TRACK_OFFSET_METHOD
				.. ": "
				.. type(offset)
		)
		return nil
	end

	return offset
end

--[[ 连接设备并初始化Modbus信号
    @param device_config 设备配置表，包含连接信息和寄存器地址
     device_config.connection：连接信息表
           connection.ip_port：IP端口号
           connection.slave_num：从机地址
     device_config.addresses：寄存器地址表
           addresses.laser_operate：激光操作寄存器地址
           addresses.job_number：线机光job号寄存器地址
           addresses.target_pose_validity：目标位姿有效性寄存器地址
           addresses.target_pose_in_device_x：设备坐标系下目标位姿X寄存器地址
           addresses.target_pose_in_device_y：设备坐标系下目标位姿Y寄存器地址
           addresses.target_pose_in_device_z：设备坐标系下目标位姿Z寄存器地址
    @return 无返回值
]]
local function connectDevice(device_config)
	local _ENV = sched.select_robot(1)

	local conn = device_config.connection
	local addr = device_config.addresses

	modbusAddSignal(conn.ip_port, conn.slave_num, addr.laser_operate, 3, "LINE_LASER_OPERATE", false)
	modbusAddSignal(conn.ip_port, conn.slave_num, addr.job_number, 3, "LINE_LASER_JOB_NUMBER", false)
	modbusAddSignal(
		conn.ip_port,
		conn.slave_num,
		addr.target_pose_validity,
		3,
		"LINE_LASER_TARGET_POSE_VALIDITY",
		false
	)
	-- 不更新x的地址(-1)，因为控制器会对下一次指令有1S延时
	-- modbusAddSignal(conn.ip_port, conn.slave_num, addr.target_pose_in_device_x, 3, 'LINE_LASER_READ_TARGET_POSE_X', false)
	modbusAddSignal(
		conn.ip_port,
		conn.slave_num,
		addr.target_pose_in_device_y,
		3,
		"LINE_LASER_READ_TARGET_POSE_Y",
		false
	)
	modbusAddSignal(
		conn.ip_port,
		conn.slave_num,
		addr.target_pose_in_device_z,
		3,
		"LINE_LASER_READ_TARGET_POSE_Z",
		false
	)
end

--[[ 打开、关闭线激光
    @param operate_value 该值为 1：打开，为 0：关闭
    @return bool true 成功,false 失败
]]
local function operateLaserStatus(operate_value)
	local _ENV = sched.select_robot(1)
	modbusSetOutputSignal("LINE_LASER_OPERATE", operate_value)

	local poll_interval = 0.1
	local timeout_period = 5.0
	local elapsed_time = 0.0
	local current_value = modbusGetSignalStatus("LINE_LASER_OPERATE")

	if current_value == operate_value then
		return true
	end

	while elapsed_time < timeout_period do
		sched.sleep(poll_interval)
		elapsed_time = elapsed_time + poll_interval
		current_value = modbusGetSignalStatus("LINE_LASER_OPERATE")
		if current_value == operate_value then
			return true
		end
	end

	log_info(welding_common.log_prefix .. " Set operate value: " .. tostring(operate_value))
	log_info(welding_common.log_prefix .. " Current station value: " .. tostring(current_value))
	log_error(welding_common.log_prefix .. " Failed to set to " .. tostring(operate_value))

	return false
end

--[[ 设置job号
    @param job_number job号
    @return bool true 设置成功, false 设置失败
]]
local function setJobNumber(job_number)
	local _ENV = sched.select_robot(1)

	modbusSetOutputSignal("LINE_LASER_JOB_NUMBER", job_number)

	local poll_interval = 0.1
	local timeout_period = 5.0
	local elapsed_time = 0.0
	local current_value = modbusGetSignalStatus("LINE_LASER_JOB_NUMBER")

	if current_value == job_number then
		return true
	end

	while elapsed_time < timeout_period do
		sched.sleep(poll_interval)
		elapsed_time = elapsed_time + poll_interval
		current_value = modbusGetSignalStatus("LINE_LASER_JOB_NUMBER")
		if current_value == job_number then
			return true
		end
	end

	log_info(welding_common.log_prefix .. " Set job number: " .. tostring(job_number))
	log_info(welding_common.log_prefix .. " Current station value: " .. tostring(current_value))
	log_error(welding_common.log_prefix .. " Failed to set LINE_LASER_JOB_NUMBER to " .. tostring(job_number))

	return false
end

--[[ 获取当前job号
    @return current_job_number 当前job号，数值由设备定义
]]
local function getJobNumber()
	local _ENV = sched.select_robot(1)
	local current_job_number = modbusGetSignalStatus("LINE_LASER_JOB_NUMBER")
	return current_job_number
end

--[[ 检查焊缝识别是否有效
    @return bool true 能识别到焊缝, false 不能识别到焊缝
]]
local function getTargetPosInDeviceValidity()
	local _ENV = sched.select_robot(1)
	local validity = modbusGetSignalStatus("LINE_LASER_TARGET_POSE_VALIDITY")

	if validity == nil then
		log_error(welding_common.log_prefix .. "Failed to read LINE_LASER_TARGET_POSE_VALIDITY")
		return false
	end

	local numeric_validity = tonumber(validity)
	if numeric_validity == nil then
		log_error(welding_common.log_prefix .. "LINE_LASER_TARGET_POSE_VALIDITY is not numeric: " .. tostring(validity))
		return false
	end

	if numeric_validity ~= 0 then
		return true
	else
		return false
	end
end

--[[ 将原始数值转换为有符号短整型（16位）
    @param value 待转换的原始数值
    @return number 转换后的有符号短整型值，范围[-32768, 32767]
]]
local function toShort(value)
	local integerValue = math.floor(value)
	return ((integerValue + 32768) % 65536) - 32768
end

--[[ 将整数值转换为带两位小数的实际值
    @param value 待转换的整数值
    @return number 转换后的实际值（除以100000）
]]
local function addDecimalPoint(value)
	return value / 100000.0
end

--[[ 读取线激光传感器识别到的焊缝目标点位置
    @return table {x, y, z, 0, 0, 0} 目标点在激光传感器坐标系下的位姿
     (x, y, z): 位置坐标，单位毫米(m), 后三位固定为0
]]
local function getTargetPosInDevice()
	local _ENV = sched.select_robot(1)

	if not getTargetPosInDeviceValidity() then
		return nil, "target pose validity is false"
	end

	if not laser_device_config or type(laser_device_config) ~= "table" then
		return nil, "laser_device_config is missing"
	end

	if type(laser_device_config.laser_offset) ~= "table" then
		return nil, "laser_offset config is missing"
	end

	-- 从 Modbus 读取原始信号值
	-- 不更新x的地址(-1)，因为控制器会对下一次指令有1S延时
	-- local origin_x = modbusGetSignalStatus('LINE_LASER_READ_TARGET_POSE_X')
	local origin_x = 0.
	local origin_y = modbusGetSignalStatus("LINE_LASER_READ_TARGET_POSE_Y")
	local origin_z = modbusGetSignalStatus("LINE_LASER_READ_TARGET_POSE_Z")

	if origin_x == nil or origin_y == nil or origin_z == nil then
		return nil, "failed to read one or more Modbus signals for target pose"
	end

	local offset_x = tonumber(laser_device_config.laser_offset.x)
	local offset_y = tonumber(laser_device_config.laser_offset.y)
	local offset_z = tonumber(laser_device_config.laser_offset.z)
	if offset_x == nil or offset_y == nil or offset_z == nil then
		return nil, "laser_offset contains non-numeric value"
	end

	local function computeAxisValue(axis_name, rawSignal, offset)
		local numeric_signal = tonumber(rawSignal)
		if numeric_signal == nil then
			return nil, axis_name .. " raw signal is not numeric"
		end

		local short_val = toShort(numeric_signal)
		return addDecimalPoint(short_val) + offset
	end

	local x, x_err = computeAxisValue("X", origin_x, offset_x)
	if x_err then
		return nil, x_err
	end
	local y, y_err = computeAxisValue("Y", origin_y, offset_y)
	if y_err then
		return nil, y_err
	end
	local z, z_err = computeAxisValue("Z", origin_z, offset_z)
	if z_err then
		return nil, z_err
	end

	return { x, y, z, 0, 0, 0 }, nil
end

handler_laser_track_record = sched.thread(function()
	while true do
		trackRecordSeekTcpPose()
		sched.sync()
	end
end, "thread_laser_track_record")

handler_laser_track_exec = sched.thread(function()
	local _ENV = sched.select_robot(1)
	while true do
		local offset = trackExpectPosOffsetInLine()
		if (offset ~= nil) and (next(offset) ~= nil) then
			pathOffsetSet(offset, 0)
		end
		sched.sync()
	end
end, "thread_laser_track_exec")

-- 导出模块
M.addLaserData = addLaserData
M.getLaserData = getLaserData
M.resetLaserData = resetLaserData
M.connectDevice = connectDevice
M.operateLaserStatus = operateLaserStatus
M.setJobNumber = setJobNumber
M.getJobNumber = getJobNumber
M.getTargetPosInDeviceValidity = getTargetPosInDeviceValidity
M.toShort = toShort
M.addDecimalPoint = addDecimalPoint
M.getTargetPosInDevice = getTargetPosInDevice
M.setHandEye = setHandEye
M.setLineLaserType = setLineLaserType
M.trackingInit = trackingInit
M.trackingDeinit = trackingDeinit
M.setFilter = setFilter
M.setPoseMatchOffsetTime = setPoseMatchOffsetTime
M.setLaserTcpCacheSize = setLaserTcpCacheSize
M.setLaserDeviceConfig = setLaserDeviceConfig
M.getLaserDeviceConfig = getLaserDeviceConfig

return M
