
dofile(LockOn_Options.script_path.."utils.lua")
dofile(LockOn_Options.script_path.."command_defs.lua")
dofile(LockOn_Options.script_path.."Systems/electric_system_api.lua")
dofile(LockOn_Options.script_path.."Systems/device_header.lua")
dofile(LockOn_Options.script_path.."devices.lua")
dofile(LockOn_Options.script_path.."cockpit_cfg.lua")

range_scale 		  	= 60000.0
TDC_range_carret_size 	= 5000

local rander_debug_info = 0

local targetnum = 300
local scan_angle = 60
local scan_height_angle = 30
local currentScanAngle = scan_angle
local currentScanHeightAngle = scan_height_angle
local scan_update_counter = 0
local base_interval = 5 -- 可根据实际体验调整
local life_time = 4
local life_time_low = 0.1

-- 头盔与HUD范围（单位：度）
local HUD_AZ_DEG = 7 --math.deg(HUD_AZ)
local HUD_EL_DEG = 7 --math.deg(HUD_EL)
local in_hud=false
--------------------------------------------------------------------------------
--【调试参数句柄声明】（请确保这些句柄在全局或者模块初始化时执行一次）
--------------------------------------------------------------------------------
--【调试参数句柄声明（带命名键的表）】
-- 各项依次表示：
-- center_az: 扫描中心-AZ
-- center_el: 扫描中心-EL
-- status: 当前状态
-- target: 当前选中目标编号
-- global_timeout: 全局计时（超时累计）
-- candidate_count: 候选目标数量
-- choice_range: 选中目标距离（km）
-- az_diff: 目标差值-AZ（deg）
-- el_diff: 目标差值-EL（deg）
-- max_scan_az: 扫描范围-maxScanAZ（deg）
-- max_scan_el: 扫描范围-maxScanEL（deg）
--------------------------------------------------------------------------------
local param_debug_acm = {
    center_az      = get_param_handle("DEBUG_ACM_CENTER_AZ"),
    center_el      = get_param_handle("DEBUG_ACM_CENTER_EL"),
    status         = get_param_handle("DEBUG_ACM_STATUS"),
    target         = get_param_handle("DEBUG_ACM_TARGET"),
    target_az      = get_param_handle("DEBUG_ACM_TARGET_AZ"),
    target_el      = get_param_handle("DEBUG_ACM_TARGET_EL"),
    global_timeout = get_param_handle("DEBUG_ACM_GLOBAL_TIMEOUT"),
    candidate_count= get_param_handle("DEBUG_ACM_CANDIDATE_COUNT"),
    choice_range   = get_param_handle("DEBUG_ACM_CHOICE_RANGE"),
    az_diff        = get_param_handle("DEBUG_ACM_AZ_DIFF"),
    el_diff        = get_param_handle("DEBUG_ACM_EL_DIFF"),
    max_scan_az    = get_param_handle("DEBUG_ACM_MAX_SCAN_AZ"),
    max_scan_el    = get_param_handle("DEBUG_ACM_MAX_SCAN_EL")
}
local param_hsi_acm_zone = get_param_handle("HSI_ACM_ZONE")
--------------------------------------------------------------------------------
---------------------------
--【调试信息参数】（聚类调试输出）
---------------------------
local debug_cluster_count       = get_param_handle("DEBUG_CLUSTER_COUNT")
local debug_cluster_latest_time = get_param_handle("DEBUG_CLUSTER_LATEST_TIME")
-- 这里新建一组 debug 参数句柄，
local param_debug_cluster = {
    center_az       = get_param_handle("DEBUG_RADAR_CLUSTER_CENTER_AZ"),
    center_el       = get_param_handle("DEBUG_RADAR_CLUSTER_CENTER_EL"),
    status          = get_param_handle("DEBUG_RADAR_CLUSTER_STATUS"),
    target          = get_param_handle("DEBUG_RADAR_CLUSTER_TARGET"),
    candidate_count = get_param_handle("DEBUG_RADAR_CLUSTER_CANDIDATE_COUNT"),
    choice_range    = get_param_handle("DEBUG_RADAR_CLUSTER_CHOICE_RANGE"),
    az_diff         = get_param_handle("DEBUG_RADAR_CLUSTER_AZ_DIFF"),
    el_diff         = get_param_handle("DEBUG_RADAR_CLUSTER_EL_DIFF"),
    max_scan_az     = get_param_handle("DEBUG_RADAR_CLUSTER_MAX_SCAN_AZ"),
    max_scan_el     = get_param_handle("DEBUG_RADAR_CLUSTER_MAX_SCAN_EL")
}
--------------------------------------------------------------------------------
--【新增：acmLock中心调试句柄】
--------------------------------------------------------------------------------
--local param_debug_acmLock_center={
--    center_az=get_param_handle("DEBUG_CENTER_AZ"),
--    center_el=get_param_handle("DEBUG_CENTER_EL"),
--    comp_center_az=get_param_handle("DEBUG_COMP_CENTER_AZ"),
--    comp_center_el=get_param_handle("DEBUG_COMP_CENTER_EL"),
--}
--------------------------------------------------------------------------------
--【新增：雷达模式调试句柄】
--------------------------------------------------------------------------------
local param_debug_radar_mode = get_param_handle("DEBUG_RADAR_MODE")

-- 头盔是否在雷达波束范围参数（用于HMD页面显示）
local helmet_in_radar_scan = get_param_handle("HELMET_IN_RADAR_SCAN")
local helmet_in_hud = get_param_handle("HELMET_IN_HUD")
local param_debug_switch = get_param_handle("DEBUG_SWITCH")
local param_debug_pitch_deg = get_param_handle("DEBUG_PITCH_DEG")
local param_debug_roll_deg = get_param_handle("DEBUG_ROLL_DEG")
local param_debug_pitch = get_param_handle("DEBUG_PITCH")
local param_debug_roll = get_param_handle("DEBUG_ROLL")
local param_debug_in_hud = get_param_handle("DEBUG_IN_HUD")
-------------------------------------------------------------
-- 1. 新建调试参数句柄（显示目标速度、预测角变化、本机速度和参考系状态）
local param_debug_velocity_disp = {
    target_vx  = get_param_handle("DEBUG_TARGET_VX"),   -- 显示目标 vx（单位：m/s）
	target_vy  = get_param_handle("DEBUG_TARGET_VY"),   -- 显示目标 vy（单位：m/s）
    target_vz  = get_param_handle("DEBUG_TARGET_VZ"),   -- 显示目标 vz（单位：m/s）
	target_head= get_param_handle("DEBUG_TARGET_HEAD"),
	target_rel_head= get_param_handle("DEBUG_TARGET_REL_HEAD"),
    pred_az    = get_param_handle("DEBUG_PRED_AZ"),       -- 显示预测 ΔAZ（单位：°）
    pred_el    = get_param_handle("DEBUG_PRED_EL"),       -- 显示预测 ΔEL（单位：°）
    self_vx    = get_param_handle("DEBUG_SELF_VX"),       -- 显示本机前向速度 Vx0（单位：m/s）
    self_vy    = get_param_handle("DEBUG_SELF_VY"),       -- 显示本机横向速度 Vy0（单位：m/s）
    self_vz    = get_param_handle("DEBUG_SELF_VZ"),       -- 显示本机垂直速度 Vz0（单位：m/s）
    self_head  = get_param_handle("DEBUG_SELF_HEAD"),     -- 显示本机航向角（单位：°）
    ref_mode   = get_param_handle("DEBUG_REF_MODE")       -- 显示参考系信息（例如 "World" 或 "Relative"）
}


local update_time_step 	= 0.02		--50次每秒

perfomance = {
	-- 简化计算，不能旋转
	roll_compensation_limits	= {-math.rad(180), math.rad(180)},
	pitch_compensation_limits	= {-math.rad(45), math.rad(45)},

	tracking_azimuth   			= { -math.rad(scan_angle),math.rad(scan_angle)}, -- available to use, set to 90 for better performance
	tracking_elevation 			= { -math.rad(scan_height_angle),math.rad(scan_height_angle)}, -- not available: only +- 30 deg limite

	-- 在当前设置下，扫描区间为5deg, 扫描周期为41/50 s。
	scan_volume_azimuth 	= math.rad(2*scan_angle), --扫描范围
	scan_volume_elevation	= math.rad(2*scan_height_angle),
	scan_beam				= math.rad(scan_height_angle*0.5), --扫描扇区纵向面积
	--scan_speed				= math.rad(scan_angle*update_time_step), -- 扫描速度

	max_available_distance  = 550000.0,--200*60000.0,
	dead_zone 				= 200.0,

	ground_clutter =
	{-- spot RCS = A + B * random + C * random 
		sea		   	   = {0 ,0,0},
		land 	   	   = {0 ,0,0},
		artificial 	   = {0 ,0,0},
		--rays_density   = 0.01,
		max_distance   = 550000,
	}

}


------------------------------------------------------------------------------

RadarSystem 	= GetSelf()
DEBUG_ACTIVE 	= false
Sensor_Data_Raw = get_base_data()


RadarSystemice_timer_dt		= 0.02

make_default_activity(update_time_step)

Radar = {
	mode_h 			= get_param_handle("RADAR_MODE"),
	-- 1:searching, 2:trying to lock, 3:STT lock			
	opt_pb_stab_h 	= get_param_handle("RADAR_PITCH_BANK_STABILIZATION"),
	opt_bank_stab_h = get_param_handle("RADAR_BANK_STABILIZATION"),
	opt_pitch_stab_h= get_param_handle("RADAR_PITCH_STABILIZATION"),
	--tdc
	tdc_azi_h 		= get_param_handle("RADAR_TDC_AZIMUTH"),
	tdc_range_h 	= get_param_handle("RADAR_TDC_RANGE"),
--	tdc_closet_h	= get_param_handle("CLOSEST_RANGE_RESPONSE"),
	tdc_rcsize_h	= get_param_handle("RADAR_TDC_RANGE_CARRET_SIZE"),
	tdc_acqzone_h   = get_param_handle("ACQUSITION_ZONE_VOLUME_AZIMUTH"),

	stt_range 		= get_param_handle("RADAR_STT_RANGE"),
	stt_azimuth_h 	= get_param_handle("RADAR_STT_AZIMUTH"),
	stt_elevation_h = get_param_handle("RADAR_STT_ELEVATION"),

	sz_azimuth_h 	= get_param_handle("SCAN_ZONE_ORIGIN_AZIMUTH"),
	sz_elevation_h 	= get_param_handle("SCAN_ZONE_ORIGIN_ELEVATION"),

	tdc_ele_up_h 	= get_param_handle("RADAR_TDC_ELEVATION_AT_RANGE_UPPER"),
	tdc_ele_down_h 	= get_param_handle("RADAR_TDC_ELEVATION_AT_RANGE_LOWER"),

	ws_ir_slave_azimuth_h	= get_param_handle("WS_IR_MISSILE_SEEKER_DESIRED_AZIMUTH"),
	ws_ir_slave_elevation_h	= get_param_handle("WS_IR_MISSILE_SEEKER_DESIRED_ELEVATION"),

	bit_h 			= get_param_handle("RADAR_BIT"),

	antenna_pos 	= get_param_handle("SCAN_BEAM_AZIMUTH"), --设置扫描扇区显示
}

------------------------------------------------------------------------------
local hmd_init_done = false

local rdr_contact_params_h = {}
local param_h_HMD_acm_targets = {}
--local acm_targets = {}
local param_h = {
	RDR_CONTACT_COUNT 		= {get_param_handle("RADAR_CONTACT_COUNT"),		0},
	tdc_range_scale 		= {get_param_handle("RADAR_TDC_RANGE_SCALE"),	0},
	tdc_azi_range 			= {get_param_handle('RADAR_TDC_AZIMUTH_RANGE'),	0},
	TARGET_BOX_DIS_ENABLE 	= {get_param_handle('TARGET_BOX_DIS_ENABLE'),	0},
	TARGET_RANGE 			= {get_param_handle('TARGET_RANGE'),			0},
	TARGET_RANGE_NM 		= {get_param_handle('TARGET_RANGE_NM'),			0},
	RADAR_STT_RANGE_SCALE 	= {get_param_handle('RADAR_STT_RANGE_SCALE'),	0},
	TARGET_BOX_X 			= {get_param_handle('TARGET_BOX_X'),			0},
	TARGET_BOX_Y 			= {get_param_handle('TARGET_BOX_Y'),			0},
	HMD_TARGET_BOX_X 		= {get_param_handle('HMD_TARGET_BOX_X'),		0},
	HMD_TARGET_BOX_Y 		= {get_param_handle('HMD_TARGET_BOX_Y'),		0},
	RADAR_BRT 				= {get_param_handle('RADAR_BRT'),				0},
	SCAN_ZONE_ORIGIN_ELEVATION_BP5 	= {get_param_handle('SCAN_ZONE_ORIGIN_ELEVATION_BP5'),0},
	SCAN_BEAM_ELEVATION 	= {get_param_handle('SCAN_BEAM_ELEVATION'),		22.5},
	RADAR_WORK_MODE 		= {get_param_handle('RADAR_WORK_MODE'),			0},
	--0:BVR,1:广角截获WACQ,2:长孔径头盔截获LACQ,3短孔径头盔截获HACQ,4垂扫模式VACQ
    TARGET_NCTR             = {get_param_handle('TARGET_NCTR'),			   "?"},
}
-- 新增：HMD插值临时数组
local hmd_display_contacts = {}
for i = 1, targetnum do
    hmd_display_contacts[i] = { azimuth = 0, elevation = 0, range = 0 ,
	vel_az = 0, vel_el = 0 -- 新增速度跟踪 
	}
end
local unit_h=get_param_handle('UNIT')

------------------------------------------------------------------------------

-- 侦听操作按键输入
RadarSystem:listen_command(Keys.PlaneRadarUp)
RadarSystem:listen_command(Keys.PlaneRadarDown)
RadarSystem:listen_command(Keys.PlaneRadarUpLeft)
RadarSystem:listen_command(Keys.PlaneRadarUpRight)
RadarSystem:listen_command(Keys.PlaneRadarDownLeft)
RadarSystem:listen_command(Keys.PlaneRadarDownRight)
RadarSystem:listen_command(Keys.PlaneRadarReset)
RadarSystem:listen_command(JoyCmd.PlaneSelecterVertical)
RadarSystem:listen_command(JoyCmd.PlaneSelecterHorizontal)
RadarSystem:listen_command(JoyCmd.PlaneSelecterVerticalAbs)
RadarSystem:listen_command(JoyCmd.PlaneSelecterHorizontalAbs)
RadarSystem:listen_command(JoyCmd.PlaneRadarVertical)
RadarSystem:listen_command(JoyCmd.PlaneRadarHorizontal)
RadarSystem:listen_command(JoyCmd.PlaneRadarVerticalAbs)
RadarSystem:listen_command(JoyCmd.PlaneRadarHorizontalAbs)
RadarSystem:listen_command(Keys.PlaneChangeLock)
RadarSystem:listen_command(Keys.SelecterLeft) --scanzone left
RadarSystem:listen_command(Keys.SelecterRight) --scanzone right
RadarSystem:listen_command(Keys.SelecterUp) --scanzone up
RadarSystem:listen_command(Keys.SelecterDown) --scanzone down
RadarSystem:listen_command(Keys.PlaneChangeRadarPRF) --change PRF (radar puls freqency)雷达脉冲频率更改
RadarSystem:listen_command(509) --lock start
RadarSystem:listen_command(510) --lock finish
RadarSystem:listen_command(Keys.setRadarStable)
RadarSystem:listen_command(Keys.STICK_SENSOR_CONTROL_AFT)
RadarSystem:listen_command(Keys.STICK_SENSOR_CONTROL_FWD)
RadarSystem:listen_command(Keys.STICK_SENSOR_CONTROL_LEFT)
RadarSystem:listen_command(Keys.STICK_SENSOR_CONTROL_RIGHT)
RadarSystem:listen_command(Keys.Plane_HOTAS_TargetManagementSwitchUp 	  )
RadarSystem:listen_command(Keys.Plane_HOTAS_TargetManagementSwitchDown 	  )
RadarSystem:listen_command(Keys.Plane_HOTAS_TargetManagementSwitchLeft 	  )
RadarSystem:listen_command(Keys.Plane_HOTAS_TargetManagementSwitchRight   )
RadarSystem:listen_command(Keys.Plane_HOTAS_TargetManagementSwitchOff 	  )

------------------------------------------------------------------------------
-- 传感器按键计时器
local keyDnTimer = {
	[Keys.STICK_SENSOR_CONTROL_AFT] = { dn = 0, time = 0 },
	[Keys.STICK_SENSOR_CONTROL_FWD] = { dn = 0, time = 0 },
	[Keys.STICK_SENSOR_CONTROL_LEFT] = { dn = 0, time = 0 },
	[Keys.STICK_SENSOR_CONTROL_RIGHT] = { dn = 0, time = 0 },
}
------------------------------------------------------------------------------

-- 雷达显示最大距离
local maxRange_h = get_param_handle("MAX_RANGE")
-- 头盔相关
local head_el = get_param_handle('HEAD_EL')
local head_az = get_param_handle('HEAD_AZ')
local head_el_rad = math.rad(head_el:get())
local head_az_rad = -math.rad(head_az:get())
-- 攻击模式
local atkMode_h = get_param_handle('ATK_MODE')
-- Hud亮度
local hud_dis=get_param_handle("HUD_BRT")
-- 雷达校准数据
local radar_scan_pos_alig = get_param_handle('radar_scan_pos_align')
local radar_scan_pos_align = 0
local radar_scan_align_target = 0
-- ACM参数
local acmRadarStatus = 0 -- 0=搜索 1=锁定中 2=已锁定
local acmTryLockIdx = 0
local acmTryLockTime = 0
local acmGlobalTimeout = 0
-- 自身状态数组
local self_status_array = {}
-- 雷达扫描结果数组
local radar_result_temp_array = {}
-- 雷达航迹跟踪数组
local radar_target_tracing_array = {}
-- 雷达扫描增稳模式状态
local radar_stable_status = 1

------------------------------------------------------------------------------
----局部函数-------------------------------------------------------------------
-- 辅助函数：如果字段是参数句柄则调用 get() 获取其值，否则直接返回字段值
local function getValue(field)
    if type(field) == "table" and field.get and type(field.get) == "function" then
        return field:get()
    else
        return field
    end
end
-- 辅助函数：构建目标列表调试信息字符串
-- 参数 list: 待打印的目标表，表中目标对象为普通表格式，其字段可能为普通值或参数句柄
-- 参数 title: 调试信息头部字符串，例如 "Radar Contacts:" 或 "Filted Contacts:"
-- 参数 prefix: 每条记录前缀，例如 "Contact" 或 "FiltedContact"
local function buildContactMessage(list, title, prefix)
    local msg = title .. "\n"
    if #list == 0 then
        msg = msg .. "None\n"
    else
        for i, contact in ipairs(list) do
            local timeValue = getValue(contact.time)
            if timeValue and timeValue > 0 then
                msg = msg .. string.format(
                    "%s %d: Elev=%.2f, Azim=%.2f, Range=%.2f\n Friendly=%d, Time=%.2f, TimeScale=%.2f\n",
                    prefix, i,
                    getValue(contact.elevation),
                    getValue(contact.azimuth),
                    getValue(contact.range),
                    getValue(contact.friendly),
                    timeValue,
                    getValue(contact.timeSc)
                )
            end
        end
    end
    return msg
end

-- 主调试打印函数
local function debug_rdr_contact_params_to_screen()
    if rander_debug_info == 0 then
        return
    end
    -- 打印雷达扫描捕获目标
    local debug_message = buildContactMessage(rdr_contact_params_h, "Radar Contacts:", "Contact")
    Log_and_print(debug_message)

    -- 打印过滤后的目标（radar_result_temp_array）
    local debug_message2 = buildContactMessage(radar_result_temp_array, "Filted Contacts:", "FiltedContact")
    Log_and_print(debug_message2)

    -- 输出聚类（或航迹追踪）目标（radar_target_tracing_array）
    local debug_message3 = buildContactMessage(radar_target_tracing_array, "Clustered Tracing Array:", "TracingTarget")
    Log_and_print(debug_message3)
end
--[[
local function debug_param_h_HMD_acm_targets_to_screen()
    local debug_message = "param_h_HMD_acm_targets:\n"
    for i, box in ipairs(param_h_HMD_acm_targets) do
        debug_message = debug_message .. string.format(
            "%d: X=%.2f, Y=%.2f, NCTR=%s,\n RangeKM=%.2f, RangeNM=%.2f, Friendly=%d, Time=%.2f \n",
            i,
            box.x:get(),
            box.y:get(),
			box.nctr:get(),
            box.range_km:get(),
			box.range_nm:get(),
            box.friendly:get(),
            box.time:get()
        )
    end
    -- 将调试信息输出到屏幕
    Log_and_print(debug_message)
end]]
-- 打印acm参数内容
local function debug_ACM_params_to_screen()
    if rander_debug_info == 0 then
        return
    end
    local debug_message = "ACM:\n"
          debug_message = debug_message .. string.format(
            "acmRadarStatus %d, acmTryLockIdx %d, acmTryLockTime %.0f \n Radar.mode_h %d",
            acmRadarStatus,
            acmTryLockIdx,
            acmTryLockTime,
			Radar.mode_h:get()
        )
    -- 将调试信息输出到屏幕
    Log_and_print(debug_message)
end
-- 在向量旋转之后，可以记录 原始向量 和 旋转后的向量，以确保变换正确
local function debugQuaternionTransform(v, q)
    if rander_debug_info == 0 then
        return
    end
    local rotated_v = Rotate_vector_by_quaternion(v, q)
    Log_and_print(string.format(
        "原始向量= (%.2f, %.2f, %.2f) | 旋转后向量= (%.2f, %.2f, %.2f)",
        v[1], v[2], v[3], rotated_v[1], rotated_v[2], rotated_v[3]
    ))
    return rotated_v
end
--------------------------------------------------------------------------------
-- 函数说明：
-- updateAndCleanTimedElements(list, decrement)
--   list: 包含各元素，每个元素含有 time 字段（该字段为参数句柄，需要用 get() 和 set() 进行读写）
--   decrement: 每帧需要减少的值（默认为 1），可根据具体情况调整
local function updateAndCleanTimedElements(list, decrement)
    decrement = decrement or update_time_step  -- 默认每帧减少 update_time_step
    -- 逆序遍历列表，方便在删除元素时不影响索引
    for i = #list, 1, -1 do
        local currentTime = list[i].time  -- 取得当前时间
        currentTime = currentTime - decrement      -- 递减时间
        list[i].time=currentTime             -- 更新该项的 time 值
        if currentTime <= 0 then
            table.remove(list, i)
        end
    end
end
-- 数据采集：从参数句柄中获取每个雷达扫描目标的数据,预先剔除掉无效的数据
local function get_radar_contacts()
  local contacts = {}

  radar_result_temp_array={}
  for i = 1, targetnum do
    local p = rdr_contact_params_h[i]
    local t = p.time:get()
    local nctr = p.NCTR:get()
    local noise = p.noise and p.noise:get() or 0
    if t > life_time_low and t < life_time and nctr and noise < 0.5 then
      local target = {
        id              = p.id:get(),
        elevation       = p.elevation:get(),
        azimuth         = p.azimuth:get(),
        azimuth_range   = p.azimuth_range:get(),
        friendly        = p.friendly:get(),
        NCTR            = nctr,
        range           = p.range:get(),
        range_display   = p.range_display:get(),
        rangeOfScale    = p.rangeOfScale:get(),
        rcs             = p.rcs:get(),
        rcs_coeff       = p.rcs_coeff:get(),
        rnd             = p.rnd:get(),
        time            = t,
        timeSc          = p.timeSc:get(),
        vx              = p.vx:get(),
        vy              = p.vy:get(),
        vz              = p.vz:get(),
        gs              = p.gs:get(),
        head            = p.head:get(),
        rel_head        = p.rel_head:get(),
        inLimit         = p.inLimit:get()
      }
      table.insert(contacts, target)
      table.insert(radar_result_temp_array,target)
    end
  end
  return contacts
end
-- 辅助函数：计算两个目标之间的“相似”距离（利用极坐标小角近似）
local function phase_distance(c1, c2)
    local d_range = math.abs(c1.range - c2.range)
    local avg_range = (c1.range + c2.range) / 2
    local d_az_rad = math.abs(c1.azimuth - c2.azimuth)
    local d_az_m = d_az_rad * avg_range
    local d_el_rad = math.abs(c1.elevation - c2.elevation)
    local d_el_m = d_el_rad * avg_range
    return math.sqrt(d_range^2 + d_az_m^2 + d_el_m^2)
end

-- 聚类函数：对目标数据进行聚类和合并，
-- 在每个聚类中直接选取最新的目标数据作为代表，并保持其原有的 id
local function clusterContacts(targets)
	local clustered = {}
	local used = {}
    radar_target_tracing_array = {}
	local distance_threshold = 1200  -- 根据实际情况设定阈值

	for i, target in ipairs(targets) do
	if not used[i] then
		local cluster = { target }
		used[i] = true
		    for j = i+1, #targets do
		        if not used[j] then
			        local other = targets[j]
			        if phase_distance(target, other) < distance_threshold then
			            table.insert(cluster, other)
			            used[j] = true
			        end
		        end
		    end
		    -- 直接选取聚类中最新的目标作为代表d
		    local latest = cluster[1]
		    for k = 2, #cluster do
		        if cluster[k].time < latest.time then
			        latest = cluster[k]
		        end
		    end
		    table.insert(clustered, latest)
		    table.insert(radar_target_tracing_array,latest)
	    end
	end
	return clustered
end


--------------------------------------------------------------------------------
--【聚类目标句柄表的动态生成与更新】
-- 句柄名称均以 "HMD_CLUSTER_TARGET_" 开头，字段包括：
-- id, range_km, nctr, elevation, azimuth, rcs, time, vx, vy, vz, x, y
--------------------------------------------------------------------------------
local hmd_cluster_target_params = {}  -- 初始化为空

local function updateClusterDisplay(clusteredTargets)
    local count = #clusteredTargets
    -- 动态生成不足的句柄
    for i = (#hmd_cluster_target_params + 1), count do
        hmd_cluster_target_params[i] = {
            id        = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_ID", i)),
            range_km  = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_RANGE_KM", i)),
            nctr      = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_NCTR", i)),
            elevation = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_ELEVATION", i)),
            azimuth   = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_AZIMUTH", i)),
            rcs       = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_RCS", i)),
            time      = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_TIME", i)),
            timeSc    = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_TIME_SCALE", i)),
            vx        = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_VX", i)),
            vy        = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_VY", i)),
            vz        = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_VZ", i)),
            x         = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_X", i)),
            y         = get_param_handle(string.format("HMD_CLUSTER_TARGET_%02d_Y", i))
        }
    end
    -- 清理多余的句柄：从索引 count+1 到 #hmd_cluster_target_params 逆序删除
    for i = #hmd_cluster_target_params, count+1, -1 do
        hmd_cluster_target_params[i].id:set(-1)
        hmd_cluster_target_params[i].range_km:set(0)
        hmd_cluster_target_params[i].nctr:set("")
        hmd_cluster_target_params[i].elevation:set(0)
        hmd_cluster_target_params[i].azimuth:set(0)
        hmd_cluster_target_params[i].rcs:set(100)
        hmd_cluster_target_params[i].time:set(0)
        hmd_cluster_target_params[i].timeSc:set(0)
        hmd_cluster_target_params[i].vx:set(0)
        hmd_cluster_target_params[i].vy:set(0)
        hmd_cluster_target_params[i].vz:set(0)
        hmd_cluster_target_params[i].x:set(0)
        hmd_cluster_target_params[i].y:set(0)
        table.remove(hmd_cluster_target_params, i)
    end
    -- 更新句柄内容
    for i = 1, count do
        local cluster = clusteredTargets[i]
        hmd_cluster_target_params[i].id:set(cluster.id)
        hmd_cluster_target_params[i].range_km:set(cluster.range / 1000)
        hmd_cluster_target_params[i].nctr:set(cluster.NCTR or "")
        hmd_cluster_target_params[i].elevation:set(cluster.elevation)
        hmd_cluster_target_params[i].azimuth:set(cluster.azimuth)
        hmd_cluster_target_params[i].rcs:set(cluster.rcs)
        hmd_cluster_target_params[i].time:set(cluster.time)
        hmd_cluster_target_params[i].timeSc:set(cluster.timeSc)
        hmd_cluster_target_params[i].vx:set(cluster.vx)
        hmd_cluster_target_params[i].vy:set(cluster.vy)
        hmd_cluster_target_params[i].vz:set(cluster.vz)
        hmd_cluster_target_params[i].x:set(cluster.x or 0)
        hmd_cluster_target_params[i].y:set(cluster.y or 0)
    end

    -- 更新聚类调试参数
    debug_cluster_count:set(count)
    if count > 0 then
        local latest_time = clusteredTargets[1].time
        for i = 2, count do
            if clusteredTargets[i].time > latest_time then
                latest_time = clusteredTargets[i].time
            end
        end
        debug_cluster_latest_time:set(latest_time)
    else
        debug_cluster_latest_time:set(0)
    end
end

--------------------------------------------------------------------------------
--【updateMFDDisplay 函数】
-- 示例实现：将聚类目标的简要信息整合成一段文本输出到参数句柄 MFD_CLUSTER_DISPLAY
--------------------------------------------------------------------------------
-- 假设最多显示 maxDisplayTargets 个聚类后的目标
local maxDisplayTargets = 10
-- 用于显示目标数量的句柄
local radarTargetCount = get_param_handle("RADAR_TARGET_COUNT")
-- 定义一个表 radarDisplayTargets，每个目标包含显示所需的各项参数句柄
local radarDisplayTargets = {}
for i = 1, maxDisplayTargets do
    radarDisplayTargets[i] = {
        range_display   = get_param_handle(string.format("RADAR_TARGET_%02d_RANGE_DISPLAY", i)),
        azimuth_display = get_param_handle(string.format("RADAR_TARGET_%02d_AZIMUTH_DISPLAY", i)),
        elevation_display = get_param_handle(string.format("RADAR_TARGET_%02d_ELEVATION_DISPLAY", i)),
        rcs_display     = get_param_handle(string.format("RADAR_TARGET_%02d_RCS_DISPLAY", i)),
        id_display      = get_param_handle(string.format("RADAR_TARGET_%02d_ID_DISPLAY", i))
    }
end
--显示更新函数：将聚类后的目标数据通过已经定义的参数句柄写入显示模块
local function updateMFDDisplay(clusteredTargets)
    local count = #clusteredTargets
    radarTargetCount:set(count)  -- 更新目标数量

    -- 删除多余的句柄：如果已有的句柄多于当前目标数，就删除多余的项
    for i = #radarDisplayTargets, count + 1, -1 do
        table.remove(radarDisplayTargets, i)
    end

    -- 补充不足的句柄（但不超过 maxDisplayTargets）
    local needed = math.min(count, maxDisplayTargets)
    for i = #radarDisplayTargets + 1, needed do
        radarDisplayTargets[i] = {
            range_display     = get_param_handle(string.format("RADAR_TARGET_%02d_RANGE_DISPLAY", i)),
            azimuth_display   = get_param_handle(string.format("RADAR_TARGET_%02d_AZIMUTH_DISPLAY", i)),
            elevation_display = get_param_handle(string.format("RADAR_TARGET_%02d_ELEVATION_DISPLAY", i)),
            rcs_display       = get_param_handle(string.format("RADAR_TARGET_%02d_RCS_DISPLAY", i)),
            id_display        = get_param_handle(string.format("RADAR_TARGET_%02d_ID_DISPLAY", i))
        }
    end

    -- 更新句柄内容
    for i = 1, needed do
        local target = clusteredTargets[i]
        radarDisplayTargets[i].range_display:set(target.range)
        radarDisplayTargets[i].azimuth_display:set(target.azimuth)
        radarDisplayTargets[i].elevation_display:set(target.elevation)
        radarDisplayTargets[i].rcs_display:set(target.rcs)  -- 直接使用目标的 rcs 字段
        radarDisplayTargets[i].id_display:set(target.id)
    end
end

--------------------------------------------------------------------------------

-- 独立的局部函数，更新 param_debug_cluster 的调试参数
local function update_debug_cluster(clusteredTargets)
    local candidate_count = #clusteredTargets
    param_debug_cluster.candidate_count:set(candidate_count)

    if candidate_count > 0 then
        local sum_az = 0
        local sum_el = 0
        for i = 1, candidate_count do
            sum_az = sum_az + clusteredTargets[i].azimuth
            sum_el = sum_el + clusteredTargets[i].elevation
        end
        local center_az = sum_az / candidate_count
        local center_el = sum_el / candidate_count

        param_debug_cluster.center_az:set(center_az)
        param_debug_cluster.center_el:set(center_el)
        param_debug_cluster.status:set(1)  -- 状态 1 表示目标数据有效

        -- 选取聚类结果中的第一个目标，作为选中目标
        local selected = clusteredTargets[1]
        param_debug_cluster.target:set(selected.id)
        param_debug_cluster.choice_range:set(selected.range / 1000)  -- 距离换算为 km

        param_debug_cluster.az_diff:set(math.abs(selected.azimuth - center_az))
        param_debug_cluster.el_diff:set(math.abs(selected.elevation - center_el))
    else
        param_debug_cluster.status:set(0)
        param_debug_cluster.center_az:set(0)
        param_debug_cluster.center_el:set(0)
        param_debug_cluster.target:set(0)
        param_debug_cluster.choice_range:set(0)
        param_debug_cluster.az_diff:set(0)
        param_debug_cluster.el_diff:set(0)
    end

    param_debug_cluster.max_scan_az:set(currentScanAngle)
    param_debug_cluster.max_scan_el:set(currentScanHeightAngle)
end
--------------------------------------------------------------------------------
--【新增函数：更新聚类目标的 X 和 Y】
-- 算法逻辑：
-- 对于每个聚类目标，
--   x = tan( target.azimuth + math.rad(head_az:get()) )
--   y = tan( target.elevation - math.rad(head_el:get()) )
-- 其中 head_az:get() 和 head_el:get() 提供头部姿态的角度（单位：度），
-- 并且头部的方位角定义与目标相反（因此对 azimuth 采用加法）。
--------------------------------------------------------------------------------
local function updateClusterXY()
    local headAz = head_az:get()  -- 头部姿态角度，单位：度
    local headEl = head_el:get()  -- 头部俯仰角，单位：度
    local headAzRad = math.rad(headAz)
    local headElRad = math.rad(headEl)
    local hmd_cluster_target_count = #hmd_cluster_target_params
    for i = 1, hmd_cluster_target_count do
        -- 从句柄中获取目标的 azimuth（弧度）和 elevation（弧度）
        local az = hmd_cluster_target_params[i].azimuth:get()
        local el = hmd_cluster_target_params[i].elevation:get()
        -- 计算相对角度：
        -- 对于方位：由于头部定义与目标相反，故采用 target.azimuth 加上头部角度
        local relAz = az + headAzRad
        -- 对于俯仰：直接 target.elevation 减去头部的俯仰角
        local relEl = el - headElRad
        -- 更新 X 和 Y
        hmd_cluster_target_params[i].x:set(math.tan(relAz))
        hmd_cluster_target_params[i].y:set(math.tan(relEl))
    end
end
--------------------------------------------------------------------------------
--【聚类更新主函数】
--------------------------------------------------------------------------------
local function updateClustering()
    local capturedTargets = get_radar_contacts()              -- 获取雷达扫描捕获目标数据
    local clusteredTargets = clusterContacts(capturedTargets) -- 对目标数据进行聚类，直接选取最新目标
    updateClusterDisplay(clusteredTargets)                    -- 更新聚类目标句柄及调试参数
    updateMFDDisplay(clusteredTargets)                        -- 更新 MFD 显示信息
	update_debug_cluster(clusteredTargets)					  -- 更新 HMD 显示debug信息
    updateClusterXY()                                         -- 更新每个聚类目标的 X 和 Y 值
    -- 可选：调试输出，例如 Log_and_print("聚类后目标数：" .. #clusteredTargets)
end
--------------------------------------------------------------------------------
--  TDC 双向更新函数 - 分别对公制与英制实现独立处理
--------------------------------------------------------------------------------

-- 公制下 TDC 参数从 param_h 到 Radar
local function updateTDCParamMetric()
    local currentMaxRange = maxRange_h:get()
    local range_ratio = 40 / currentMaxRange
    Radar.tdc_rcsize_h:set(5000 / range_ratio)
    Radar.tdc_range_h:set(param_h.tdc_range_scale[2] / range_ratio)
    param_h.tdc_azi_range[2] = math.tan(Radar.tdc_azi_h:get()) * param_h.tdc_range_scale[2]
end

-- 英制下 TDC 参数从 param_h 到 Radar
-- 为了使 tdc 与目标重合，我们将输入的物理距离先乘以1.852转换，再按比例处理
local function updateTDCParamImperial()
    local currentMaxRange = maxRange_h:get()
    local range_ratio = 40 / currentMaxRange
    Radar.tdc_rcsize_h:set(5000 / range_ratio)
    Radar.tdc_range_h:set(param_h.tdc_range_scale[2] / (range_ratio/1.852))
    param_h.tdc_azi_range[2] = math.tan(Radar.tdc_azi_h:get()) * (param_h.tdc_range_scale[2] * 1.852)
end

local function update_tdc_param_h_to_Radar()
    if unit_h:get() == 0 then
        updateTDCParamMetric()
    else
        updateTDCParamImperial()
    end
end

-- 公制下 TDC 数值从 Radar 同步到 param_h（旧逻辑）
local function updateTDCRadarToParamMetric()
    local currentMaxRange = maxRange_h:get()
    local radarRange = Radar.tdc_range_h:get()
    local new_tdc_range_scale = radarRange * 40 / currentMaxRange
    param_h.tdc_range_scale[2] = new_tdc_range_scale
    local new_tdc_azi_range = math.tan(Radar.tdc_azi_h:get()) * new_tdc_range_scale
    param_h.tdc_azi_range[2] = new_tdc_azi_range
end

-- 英制下，将 Radar 上的 tdc raw 数值还原为物理值
local function updateTDCRadarToParamImperial()
    local currentMaxRange = maxRange_h:get()
    local radarRange = Radar.tdc_range_h:get()
    local new_tdc_range_scale = radarRange * 40 / currentMaxRange
    -- 由于英制下 Radar.tdc_range_h 原来是 param_h.tdc_range_scale[2] 除以1.852,
    -- 逆转换时还原物理值：乘回1.852
    new_tdc_range_scale = new_tdc_range_scale / 1.852
    param_h.tdc_range_scale[2] = new_tdc_range_scale
    local new_tdc_azi_range = math.tan(Radar.tdc_azi_h:get()) * new_tdc_range_scale
    param_h.tdc_azi_range[2] = new_tdc_azi_range
end

local function update_tdc_Radar_to_param_h()
    if unit_h:get() == 0 then
        updateTDCRadarToParamMetric()
    else
        updateTDCRadarToParamImperial()
    end
end

--------------------------------------------------------------------------------
-- 2. 目标距离转换处理函数
--------------------------------------------------------------------------------
-- 公制处理（UNIT=0）：完全使用旧方案的逻辑，即直接用目标原始距离（单位 m）乘以缩放因子，
-- 显示模块内部除以1000得到的结果即为 km。
local function processTargetMetric(contact)
    local maxRange = maxRange_h:get()  -- 在公制下 maxRange_h 表示 km
    if maxRange > 0 then
         local scale = 40 / maxRange
         contact.rangeOfScale:set(contact.range:get() * scale)
         contact.range_display:set(contact.range:get() / 1000)  -- 显示为 km
    end
end

-- 英制处理（UNIT=1）：首先将目标距离由米转换为海里（1 NM = 1852 m），再乘以1000，
-- 使得显示模块除以1000后得到的数字即为 NM，再乘以缩放因子。
local function processTargetImperial(contact)
    local maxRange = maxRange_h:get()  -- 在英制下 maxRange_h 表示 NM
    if maxRange > 0 then
         local scale = 40 / maxRange
         contact.rangeOfScale:set(contact.range:get()* scale / 1.852)  -- raw 值使显示模块输出 NM
         contact.range_display:set(contact.range:get() / 1852) -- 得到海里数         
    end
end

-- 更新目标框
local function updateTargetBox()
    if Radar.mode_h:get() == 3 then
        param_h.TARGET_BOX_DIS_ENABLE[2]=hud_dis:get()
		param_h.TARGET_RANGE[2]=Radar.stt_range:get()/1000
		param_h.TARGET_RANGE_NM[2]=Radar.stt_range:get()*Meter_TO_NM
        param_h.TARGET_BOX_X[2]=math.tan(Radar.stt_azimuth_h:get())*HUD_distance
        param_h.TARGET_BOX_Y[2]=math.tan(Radar.stt_elevation_h:get())*HUD_distance

		--HMD
		param_h.HMD_TARGET_BOX_X[2]=math.tan(Radar.stt_azimuth_h:get()-math.rad(-head_az:get()))*1
        param_h.HMD_TARGET_BOX_Y[2]=math.tan(Radar.stt_elevation_h:get()-math.rad(head_el:get()))*1
    else
		param_h.TARGET_BOX_DIS_ENABLE[2]=0
		param_h.TARGET_RANGE[2]=0
        param_h.TARGET_BOX_X[2]=0
        param_h.TARGET_BOX_Y[2]=0
		--HMD
		param_h.HMD_TARGET_BOX_X[2]=0
        param_h.HMD_TARGET_BOX_Y[2]=0
    end
end

--更新扫描区波束俯仰外部参数
--从Radar.sz_elevation_h获取值转换为角度赋予scan_el；
--再将scan_el转换为弧度赋予SCAN_ZONE_ORIGIN_ELEVATION_BP5
local function updateBP5()
	local scan_el = math.deg(Radar.sz_elevation_h:get())
	param_h.SCAN_ZONE_ORIGIN_ELEVATION_BP5[2] = math.rad(scan_el*(9.6338/30))
end

-- 设置雷达增稳状态
local function setRadarStable(stab_signal)
	if stab_signal == 1 then
		stab_signal = true
	else
		stab_signal = false
	end
	if stab_signal == true then
		-- 增稳打开，用于远程扫描跟踪
		Radar.opt_pb_stab_h:set(1)
		Radar.opt_pitch_stab_h:set(1)
		Radar.opt_bank_stab_h:set(1)
		radar_stable_status = 1
	else
		-- 增稳关闭，用于近距离格斗
		Radar.opt_pb_stab_h:set(0)
		Radar.opt_pitch_stab_h:set(0)
		Radar.opt_bank_stab_h:set(0)
		radar_stable_status = 0
	end
end
-------------------------------------------------------------
-- 补偿函数：compensate_for_attitude
-------------------------------------------------------------
-- 功能：将输入的 (az, el)（单位：弧度）转换为驾驶员实际看到的视角
-- 过程：将输入角度转换为3D向量，再利用飞机姿态（roll, pitch）的反向旋转（即 -roll, -pitch）
--      来消除机体干扰，最后用透视投影换算为最终视角
local function compensate_for_attitude(az, el, roll, pitch)
    -- 将 (az, el) 转换为单位向量（假定 x 为正前方，y 为右侧，z 为上）
    local v = { math.cos(el) * math.cos(az),
                math.cos(el) * math.sin(az),
                math.sin(el) }
    -- 使用 -roll 与 -pitch 构造补偿四元数，消除机体姿态影响
    local q_comp = Quat_from_euler(roll, pitch, 0)
    local v_new = Rotate_vector_by_quaternion(v, q_comp)
    -- 由旋转后向量使用透视投影公式计算视野角度：
    local view_az, view_el = Vector_to_view_angles(v_new)
    return view_az, view_el
end

-------------------------------------------------------------
--【独立函数】LockValid
-- 判断目标是否满足锁定条件：
-- 1. 目标角度偏差（az_diff, el_diff）在规定范围内（maxScanAZ_rad、maxScanEL_rad）
-- 2. 目标距离小于 acmmaxRange
-- 3. 目标非友军
-------------------------------------------------------------
local function LockValid(t, az_diff, el_diff, maxScanAZ_rad, maxScanEL_rad, acmmaxRange)
    if az_diff <= maxScanAZ_rad and
       el_diff <= maxScanEL_rad and
       t.range:get() < acmmaxRange and
       t.friendly:get() ~= 1 then
        return true
    else
        return false
    end
end
--------------------------------------------------------------------------------
--【acmLock函数】
-- 参数 acmmaxRange、maxScanAZ、maxScanEL 均来自调用处
-- 在 HUD 模式下，所有扫描模式均以 HUD 中心 (0,0) 为锁定基准；
-- 非 HUD 模式下，只有 BST 模式（孔径或长孔径，即 work_mode==2或3）需要考虑头盔瞄准；
-- 其他模式（BVR、WACQ、VACQ、HACQ）默认以 (0,0) 为锁定中心。
--------------------------------------------------------------------------------
local function acmLock(acmmaxRange, maxScanAZ, maxScanEL)
    -- 获取当前飞机的 roll 与 pitch
    local current_roll = Sensor_Data_Raw.getRoll() or 0
    local current_pitch = Sensor_Data_Raw.getPitch() or 0
    -- 获取扫描中心（使用全局头盔角度 head_az_rad、head_el_rad）
    local center_az, center_el = head_az_rad, head_el_rad
    param_debug_acm.center_az:set(math.deg(center_az))
    param_debug_acm.center_el:set(math.deg(center_el))
    param_hsi_acm_zone:set(math.deg(center_az))

    -- 根据传入的 maxScanAZ、maxScanEL 计算扫描范围（乘以opt_scale 比例）
    local opt_scale = 1
    local maxScanAZ_rad = math.rad(maxScanAZ * opt_scale)
    local maxScanEL_rad = math.rad(maxScanEL * opt_scale)
    -- 将转换后的扫描范围（以deg为单位）写入调试句柄
    param_debug_acm.max_scan_az:set(math.deg(maxScanAZ_rad))
    param_debug_acm.max_scan_el:set(math.deg(maxScanEL_rad))

    if Radar.mode_h:get() == 3 then
        acmRadarStatus = 2
        param_debug_acm.status:set(acmRadarStatus)
    --	Log_and_print(">> 锁定成功！ <<")
        param_h.TARGET_NCTR[2] = rdr_contact_params_h[acmTryLockIdx].NCTR:get()
        return
    end
    param_debug_acm.status:set(acmRadarStatus)



    if acmRadarStatus == 0 then
        local candidates = {}
        acmTryLockIdx = 0
        param_debug_acm.target:set(acmTryLockIdx)

        -- 获取目标扫描
        for i = 1, targetnum do
            local contact = rdr_contact_params_h[i]
            if contact and contact.time:get() >= 0.799 and contact.time:get() < 2.08 then
                local radar_az = contact.azimuth:get()
                local radar_el = contact.elevation:get()
                -- 对目标角度同时进行 roll 和 pitch 补偿
                --local comp_center_az, comp_center_el = compensate_for_attitude(center_az, center_el, current_roll, current_pitch)
                local comp_target_az, comp_target_el = compensate_for_attitude(radar_az, radar_el, current_roll, current_pitch)
                local az_diff = math.abs(comp_target_az - center_az)
                local el_diff = math.abs(comp_target_el - center_el)

            --    Log_and_print(string.format( "目标%d | 补偿后中心差: 方位%.1fdeg 俯仰%.1fdeg", i, math.deg(az_diff), math.deg(el_diff)))

                if LockValid(contact, az_diff, el_diff, maxScanAZ_rad, maxScanEL_rad, acmmaxRange) then
                    table.insert(candidates, {
                        index = i,
                        range = contact.range:get(),
                        az_diff = az_diff,
                        el_diff = el_diff
                    })
                end
            end
        end
        local candidatesnumber = #candidates
        param_debug_acm.candidate_count:set(candidatesnumber)

        table.sort(candidates, function(a, b)
            return a.range < b.range
        end)

        if candidatesnumber > 0 then
            acmTryLockIdx = candidates[1].index
            local contact = rdr_contact_params_h[acmTryLockIdx]
            param_debug_acm.target:set(acmTryLockIdx)
            param_debug_acm.target_az:set(math.deg(contact.azimuth:get()))
            param_debug_acm.target_el:set(math.deg(contact.elevation:get()))
            param_debug_acm.choice_range:set(contact.range:get() / 1000)
            param_debug_acm.az_diff:set(math.deg(candidates[1].az_diff))
            param_debug_acm.el_diff:set(math.deg(candidates[1].el_diff))

            --Log_and_print(string.format( "选择目标%d | 距离=%.1fkm 方位=%.1fdeg 俯仰=%.1fdeg", acmTryLockIdx, contact.range:get() / 1000, math.deg(contact.azimuth:get()), math.deg(contact.elevation:get()) ))

            Radar.tdc_azi_h:set(contact.azimuth:get())
            Radar.tdc_range_h:set(contact.range:get())
            dispatch_action(nil, 509, 1)  -- 开始锁定
            update_tdc_Radar_to_param_h()
            acmRadarStatus = 1
            acmTryLockTime = 0
            acmGlobalTimeout = 0
            param_debug_acm.status:set(acmRadarStatus)
        else
            --Log_and_print("未找到有效目标")
            acmGlobalTimeout = acmGlobalTimeout + update_time_step
            param_debug_acm.global_timeout:set(acmGlobalTimeout)
            if acmGlobalTimeout > 1 then
                dispatch_action(nil, 510, 1)
                acmTryLockIdx = 0
                --Log_and_print("全局超时，重置扫描周期")
                acmGlobalTimeout = 0
                param_debug_acm.global_timeout:set(acmGlobalTimeout)
                param_debug_acm.target:set(acmTryLockIdx)
            end
        end

    elseif acmRadarStatus == 1 then
        local current_center_az, current_center_el = head_az_rad, head_el_rad
        local contact = rdr_contact_params_h[acmTryLockIdx]
        local valid_scale = 1
        local validScanAZ_rad = maxScanAZ_rad * valid_scale
        local validScanEL_rad = maxScanEL_rad * valid_scale

        local org_az_valid = math.abs(contact.azimuth:get() - current_center_az) <= validScanAZ_rad
        local org_el_valid = math.abs(contact.elevation:get() - current_center_el) <= validScanEL_rad
        --Log_and_print(string.format("[动态检查]有效目标：%s\n当前中心差: 方位%.1fdeg 俯仰%.1fdeg",org_az_valid and org_el_valid and "是" or "否",math.deg(math.abs(contact.azimuth:get() - current_center_az)),math.deg(math.abs(contact.elevation:get() - current_center_el))))

        local comp_current_center_az, comp_current_center_el = compensate_for_attitude(current_center_az, current_center_el, current_roll, current_pitch)
        local comp_contact_az, comp_contact_el = compensate_for_attitude(contact.azimuth:get(), contact.elevation:get(), current_roll, current_pitch)
        local az_valid = math.abs(comp_contact_az - comp_current_center_az) <= validScanAZ_rad
        local el_valid = math.abs(comp_contact_el - comp_current_center_el) <= validScanEL_rad
        --Log_and_print(string.format("[动态检查]有效目标：%s\n 补偿后当前中心差: 方位%.1fdeg 俯仰%.1fdeg",az_valid and el_valid and "是" or "否",math.deg(math.abs(comp_contact_az - comp_current_center_az)),math.deg(math.abs(comp_contact_el - comp_current_center_el))))

        if not (az_valid and el_valid) then
            dispatch_action(nil, 510, 1)
            acmRadarStatus = 0
            --Log_and_print("!! 目标移出动态扫描区 !!")
            param_debug_acm.status:set(acmRadarStatus)
        end

        acmTryLockTime = acmTryLockTime + update_time_step
        acmGlobalTimeout = acmGlobalTimeout + update_time_step
        param_debug_acm.global_timeout:set(acmGlobalTimeout)
        --Log_and_print(string.format("[锁定中] 已耗时%.1f秒 | 全局计时%.1fs",acmTryLockTime, acmGlobalTimeout))

        if acmGlobalTimeout > 1.5 then
            dispatch_action(nil, 510, 1)
            acmRadarStatus = 0
            param_debug_acm.status:set(acmRadarStatus)
            --Log_and_print("!! 全局超时强制重置 !!")
            return
        end

        if contact and contact.time:get() >= 0.1 then
            Radar.tdc_azi_h:set(contact.azimuth:get())
            Radar.tdc_range_h:set(contact.range:get())
            dispatch_action(nil, 509, 1)
            update_tdc_Radar_to_param_h()

            if Radar.mode_h:get() == 3 then
                acmRadarStatus = 2
                param_debug_acm.status:set(acmRadarStatus)
                --Log_and_print(">> 锁定成功 <<")
                return
            end

            local current_az = contact.azimuth:get()
            local current_el = contact.elevation:get()

            local range_valid = contact.range:get() < acmmaxRange * 1.1

            org_az_valid = math.abs(current_az - head_az_rad) <= maxScanAZ_rad * 1.2
            org_el_valid = math.abs(current_el - head_el_rad) <= maxScanEL_rad * 1.2
            --Log_and_print(string.format("目标检查 | 方位有效=%s 俯仰有效=%s 距离有效=%s",org_az_valid and "是" or "否",org_el_valid and "是" or "否",range_valid and "是" or "否"))

            local comp_current_az, comp_current_el = compensate_for_attitude(current_az, current_el, current_roll, current_pitch)
            local comp_head_az, comp_head_el = compensate_for_attitude(head_az_rad, head_el_rad, current_roll, current_pitch)
            az_valid = math.abs(comp_current_az - comp_head_az) <= maxScanAZ_rad * 1.2
            el_valid = math.abs(comp_current_el - comp_head_el) <= maxScanEL_rad * 1.2
            --Log_and_print(string.format("目标检查 | 补偿后方位有效=%s 俯仰有效=%s 距离有效=%s",az_valid and "是" or "否",el_valid and "是" or "否",range_valid and "是" or "否"))
            if not (az_valid and el_valid and range_valid) then
                dispatch_action(nil, 510, 1)
                acmRadarStatus = 0
                param_debug_acm.status:set(acmRadarStatus)
                --Log_and_print("!! 目标失效，中止锁定 !!")
            end
        else
            dispatch_action(nil, 510, 1)
            acmRadarStatus = 0
            param_debug_acm.status:set(acmRadarStatus)
            param_h.TARGET_NCTR[2] = nil
            --Log_and_print("!! 目标丢失，重置状态 !!")
        end

    elseif acmRadarStatus == 2 then
        if Radar.mode_h:get() ~= 3 then
            acmRadarStatus = 0
            param_debug_acm.status:set(acmRadarStatus)
            --Log_and_print("!! 雷达脱锁，返回搜索状态 !!")
        else
            if unit_h:get() == 0 then
				local debug_message=string.format("[锁定维持] 当前目标%d | 距离=%.1fkm",acmTryLockIdx,rdr_contact_params_h[acmTryLockIdx].range:get() / 1000)
                --Log_and_print(debug_message)
            else
				local debug_message=string.format("[锁定维持] 当前目标%d | 距离=%.1fnm",acmTryLockIdx,rdr_contact_params_h[acmTryLockIdx].range:get() * Meter_TO_NM)
                --Log_and_print(debug_message)
            end
        end
    end
end
--------------------------------------------------------------------------------
-- 广角扫描逻辑
local function updateWACQ()
	if param_h.RDR_CONTACT_COUNT[2]>0 then
        local acmmaxRange = 3.7040e4 --1.852e4
		acmLock(acmmaxRange,10,10) -- 最大范围 20NM，扫描范围 10° x 10°
	end
end
-- 孔径扫描逻辑
local function updateBst()
	if param_h.RDR_CONTACT_COUNT[2]>0 then
		local acmmaxRange = 0
		if param_h.RADAR_WORK_MODE[2] == 2 then
		 	acmmaxRange = 1.0186e6 -- 最大范围 550NM，扫描范围 5° x 5°
		elseif param_h.RADAR_WORK_MODE[2] == 3 then
			acmmaxRange = 3.7040e4 -- 最大范围 20NM，扫描范围 2° x 2°
		end
        acmLock(acmmaxRange,
            param_h.RADAR_WORK_MODE[2] == 2 and 5 or 2,  -- AZ
            param_h.RADAR_WORK_MODE[2] == 2 and 5 or 2   -- EL
        )
	end
end
-- 垂直扫描逻辑
local function updateVACQ()
	if param_h.RDR_CONTACT_COUNT[2]>0 then
        local acmmaxRange = 0.926e4
		acmLock(acmmaxRange,8,90) -- 最大范围 5NM，扫描范围 8° x 90°
	end
end
-- 水平扫描逻辑
local function updateHACQ()
	if param_h.RDR_CONTACT_COUNT[2]>0 then
        local acmmaxRange = 0.926e5
		acmLock(acmmaxRange,120,10) -- 最大范围 50NM，扫描范围 120° x 10°
	end
end
-- BVR扫描逻辑
local function updateRadarScan()
	--获取参数
	local scanBeamEl = param_h.SCAN_BEAM_ELEVATION[2]
	if (Radar.mode_h:get() == 1) then --雷达处于扫描模式下Radar.mode_h=1
		if (radar_scan_pos_align < currentScanAngle-1 and radar_scan_align_target == 0) then
			radar_scan_pos_align = radar_scan_pos_align + 1
			for i = 1, targetnum do
				if rdr_contact_params_h[i].time:get()<=0 then                               --time 是递增的
					rdr_contact_params_h[i].timeSc:set(0)
				else
					rdr_contact_params_h[i].timeSc:set(1-rdr_contact_params_h[i].time:get()*0.1)      --timeSc是从1自减至0
				end
				if math.abs(rdr_contact_params_h[i].azimuth:get())<math.rad(currentScanAngle) and math.abs(rdr_contact_params_h[i].elevation:get())<math.rad(currentScanHeightAngle) then
					rdr_contact_params_h[i].inLimit:set(1)
				else
					rdr_contact_params_h[i].inLimit:set(0)
				end
			end
		elseif (radar_scan_pos_align >=currentScanAngle-1 and radar_scan_align_target == 0) then
			radar_scan_align_target = 1
			scanBeamEl = scanBeamEl - 15
			update_scan_result() -- 这里说明一个周期结束，开始刷新这一轮扫描数据

		elseif (radar_scan_pos_align > -currentScanAngle and radar_scan_align_target == 1) then
			radar_scan_pos_align = radar_scan_pos_align - 1
			for i = 1, targetnum do
				if rdr_contact_params_h[i].time:get()<=0 then
					rdr_contact_params_h[i].timeSc:set(0)
				else
					rdr_contact_params_h[i].timeSc:set(1-rdr_contact_params_h[i].time:get()*0.1)
				end
				if math.abs(rdr_contact_params_h[i].azimuth:get())<math.rad(currentScanAngle) and math.abs(rdr_contact_params_h[i].elevation:get())<math.rad(currentScanHeightAngle) then
					rdr_contact_params_h[i].inLimit:set(1)
				else
					rdr_contact_params_h[i].inLimit:set(0)
				end
			end
		elseif (radar_scan_pos_align <= -currentScanAngle and radar_scan_align_target == 1) then
			radar_scan_align_target = 0
			scanBeamEl = scanBeamEl - 15
			update_scan_result() -- 这里说明一个周期结束，开始刷新这一轮扫描数据

		end
	elseif Radar.mode_h:get() == 2 then
		radar_scan_pos_align = Radar.tdc_azi_h:get()

    -- 若进入单目标锁定或准备锁定模式则重新教准
	elseif Radar.mode_h:get() == 3 then
		radar_scan_pos_align = 0
		scanBeamEl = (currentScanHeightAngle-7.5)
		radar_scan_align_target = 0
		for i = 1, targetnum do
			rdr_contact_params_h[i].timeSc:set(0)
		end
	end
	if scanBeamEl < -(currentScanHeightAngle-7.5) then
		scanBeamEl = (currentScanHeightAngle-7.5)
	end
	--参数回写
	param_h.SCAN_BEAM_ELEVATION[2] = scanBeamEl
end
--------------------------------------------------------------------------------
-- HOTAS传感器按键逻辑更新函数
-- 此函数遍历按键状态计时器 (keyDnTimer)，在雷达处于工作状态且攻击模式激活时，根据
-- 按键被按下的持续时间和释放状态来选择不同的雷达工作模式以及调整扫描角度与稳定性设置。
local function updateSensorHotas()
    -- 遍历每个按键状态:
    -- k: 按键的标识
    -- v: 与该按键关联的数据，包括dn（当前按下程度）和time（累计按下时间）
    for k, v in pairs(keyDnTimer) do
        -- 当且仅当雷达扫描模式且空对空攻击模式均激活时，响应按键
        if  Radar.mode_h:get() == 1 and atkMode_h:get() == 1 then
            -- 若按键持续被按下（按下程度大于阈值 0.9），累加累计时间
            if v.dn > 0.9 then
                v.time = v.time + update_time_step
            -- 当按键已经释放且之前有累计按键时间时
            elseif v.time > 0 and v.dn == 0 then
                -- 根据不同按键的标识分别处理
                if k == Keys.STICK_SENSOR_CONTROL_FWD then
                    -- 前推按键控制：区分长按与短按
                    if v.time > 0.8 then
                        -- 长按（超过0.8秒）：切换到BST长孔径模式
                        param_h.RADAR_WORK_MODE[2] = 2
                        currentScanAngle = 2.5
                        currentScanHeightAngle = 2.5

                        --Log_and_print("切换到BST长孔径模式")
                    else
                        -- 短按（不足0.8秒）：切换到BST短孔径模式
                        param_h.RADAR_WORK_MODE[2] = 3
                        currentScanAngle = 2
                        currentScanHeightAngle = 2

                        --Log_and_print("切换到BST短孔径模式")
                    end
                elseif k == Keys.STICK_SENSOR_CONTROL_AFT then
					if v.time > 0.8 then
					-- 长按（超过0.8秒）后拉按键：直接切换到HACQ模式（水平模式）
						param_h.RADAR_WORK_MODE[2] = 5
						currentScanAngle = 60
						currentScanHeightAngle = 5

						--Log_and_print("切换到HACQ模式")
					else
					-- 短按（不足0.8秒）后拉按键：直接切换到VACQ模式（垂直模式）
						param_h.RADAR_WORK_MODE[2] = 4
						currentScanAngle = 2
						currentScanHeightAngle = 45

						--Log_and_print("切换到VACQ模式")
					end
                elseif k == Keys.STICK_SENSOR_CONTROL_LEFT then
                    -- 左侧按键：直接切换到WACQ模式（广角模式）
                    param_h.RADAR_WORK_MODE[2] = 1
                    currentScanAngle = 5
                    currentScanHeightAngle = 5

                    --Log_and_print("切换到WACQ模式")
                end
                setRadarStable(1)   -- 雷达稳定
                -- 无论触发哪种操作，处理完当前按键后均重置累计时间
                v.time = 0
            end
        else
            -- 如果未处于雷达扫描模式或攻击模式
            -- 则重置该按键累计时间，并恢复默认扫描设置
            v.time = 0
            setRadarStable(1)  -- 恢复雷达为稳定状态
            currentScanAngle = scan_angle           -- 恢复默认扫描角度
            currentScanHeightAngle = scan_height_angle -- 恢复默认扫描高度角
        end
    end
end
--------------------------------------------------------------------------------
-- 初始化雷达扫描表
local function init_rdr_contact_params_h()
	for i = 1, targetnum do
		rdr_contact_params_h[i] = {
			elevation 		= get_param_handle(string.format('RADAR_CONTACT_%02d_ELEVATION', i)),
			azimuth 		= get_param_handle(string.format('RADAR_CONTACT_%02d_AZIMUTH', i)),
			azimuth_range 	= get_param_handle(string.format('RADAR_CONTACT_%02d_AZIMUTH_RANGE', i)),
			friendly 		= get_param_handle(string.format('RADAR_CONTACT_%02d_FRIENDLY', i)),
			id 				= get_param_handle(string.format('RADAR_CONTACT_%02d_ID', i)),
			NCTR 			= get_param_handle(string.format('RADAR_CONTACT_%02d_NCTR', i)),
			range 			= get_param_handle(string.format('RADAR_CONTACT_%02d_RANGE', i)),
			range_display   = get_param_handle(string.format('RADAR_CONTACT_%02d_RANGE_DISPLAY', i)),
			rangeOfScale 	= get_param_handle(string.format('RADAR_CONTACT_%02d_RANGE_SCALE', i)),
			rcs 			= get_param_handle(string.format('RADAR_CONTACT_%02d_RCS', i)),
			rcs_coeff 		= get_param_handle(string.format('RADAR_CONTACT_%02d_RCS_COEFF', i)),
			rnd 			= get_param_handle(string.format('RADAR_CONTACT_%02d_RND', i)),
			time 			= get_param_handle(string.format('RADAR_CONTACT_%02d_TIME', i)),
			timeSc 			= get_param_handle(string.format('RADAR_CONTACT_%02d_TIME_SCALE', i)),
			vx 				= get_param_handle(string.format('RADAR_CONTACT_%02d_VX', i)),
			vy 				= get_param_handle(string.format('RADAR_CONTACT_%02d_VY', i)),
			vz 				= get_param_handle(string.format('RADAR_CONTACT_%02d_VZ', i)),
			gs 				= get_param_handle(string.format('RADAR_CONTACT_%02d_GS', i)),
			head 			= get_param_handle(string.format('RADAR_CONTACT_%02d_HEAD', i)),
			rel_head		= get_param_handle(string.format('RADAR_CONTACT_%02d_REL_HEAD', i)),
			inLimit 		= get_param_handle(string.format('RADAR_CONTACT_%02d_IN_SC_LIMIT', i)),
            noise 			= get_param_handle(string.format('RADAR_CONTACT_%02d_NOISE', i)),
		}
	end
end
--------------------------------------------------------------------------------

-- 初始化HMD目标参数句柄
local function init_HMD_acm_targets()
    -- 预先注册所有HMD参数句柄
    for i = 1, targetnum do
		param_h_HMD_acm_targets[i] = {
			x 				= get_param_handle(string.format('HMD_CONTACT_%02d_X', 		i)),
			y 				= get_param_handle(string.format('HMD_CONTACT_%02d_Y', 		i)),
			nctr 			= get_param_handle(string.format('HMD_CONTACT_%02d_NCTR', 	i)),
			range_km 		= get_param_handle(string.format('HMD_CONTACT_%02d_RANGE_KM', i)),
			range_nm 		= get_param_handle(string.format('HMD_CONTACT_%02d_RANGE_NM', i)),
			friendly 		= get_param_handle(string.format('HMD_CONTACT_%02d_FRIENDLY', i)),
			time 			= get_param_handle(string.format('HMD_CONTACT_%02d_TIME', 	i)),
            timeSc 			= get_param_handle(string.format('HMD_CONTACT_%02d_TIME_SCALE', 	i)),
            id              = get_param_handle(string.format('HMD_CONTACT_%02d_ID', 	i)),
		}
    end
	-- 初始化检测
	if not hmd_init_done then
		for i = 1, targetnum do
			local t = rdr_contact_params_h[i]
			if t.time:get() > 0 then
				hmd_display_contacts[i].azimuth = t.azimuth:get()
				hmd_display_contacts[i].elevation = t.elevation:get()
				hmd_display_contacts[i].range = t.range:get()
			end
		end
		hmd_init_done = true
		return
	end
end

-- 3. 更新调试数据函数：使用第1个目标数据作为调试示例
local function update_debug_target_data()
    local DT = update_time_step or 0.05  -- 时间步（单位：秒）

    -- 读取第1个目标数据（外部雷达程序已更新目标数据）
    local contact = rdr_contact_params_h[1]
    if not contact then return end

    local target_vx = contact.vx:get() or 0  -- 目标 vx（单位为 m/s）
	local target_vy = contact.vy:get() or 0  -- 目标 vy（单位为 m/s）
    local target_vz = contact.vz:get() or 0  -- 目标 vz（单位为 m/s）
    local target_rng = contact.range:get() or 1  -- 目标距离（单位：米），防止除零
	local target_head= contact.head:get()	-- 目标 航向（度）
	local target_rel_head= contact.rel_head:get() -- 目标 相对航向（度）

    -- 获取本机当前速度分量，通过 sensor_data.getSelfVelocity()
    local Vx0, Vy0, Vz0 = Sensor_Data_Raw.getSelfVelocity()  -- 单位 m/s
    local ownHeading = get_param_handle('HDG'):get()  -- 本机航向（单位：角度）
    -- 更新本机速度调试参数
    param_debug_velocity_disp.self_vx:set(Vx0)
    param_debug_velocity_disp.self_vy:set(Vy0)
    param_debug_velocity_disp.self_vz:set(Vz0)
    param_debug_velocity_disp.self_head:set(ownHeading)

    -- 如果目标速度数据表示的是世界速度，则有效水平速度应为：
    local effective_vx = target_vx - Vx0
	local effective_vz = target_vz - Vz0
    -- 假设目标垂直速度直接使用 target_vy（本机速度对垂直无影响）
	local effective_vy = target_vy


    -- 计算预测角增量，简单模型: Δ = (有效速度 / 目标距离) * DT  （单位：弧度）
    local delta_az_rad = (effective_vx / target_rng) * DT
    local delta_el_rad = (effective_vy / target_rng) * DT

    -- 限制预测角增量，防止因目标速度过大导致角跳变，设定每帧最大角增量为 60°/s 转为 rad
    local MAX_PRED_RAD = math.rad(60) * DT
    if delta_az_rad > MAX_PRED_RAD then delta_az_rad = MAX_PRED_RAD end
    if delta_az_rad < -MAX_PRED_RAD then delta_az_rad = -MAX_PRED_RAD end
    if delta_el_rad > MAX_PRED_RAD then delta_el_rad = MAX_PRED_RAD end
    if delta_el_rad < -MAX_PRED_RAD then delta_el_rad = -MAX_PRED_RAD end

    local pred_delta_az_deg = math.deg(delta_az_rad)
    local pred_delta_el_deg = math.deg(delta_el_rad)

    -- 判断参考系：如果本机前向速度 (Vx0) 非零，则认为目标数据需要本机速度补偿，参考系为 "World"
    local ref_mode_str = (math.abs(Vx0) > 1) and "World" or "Relative"

    -- 更新目标速度和预测角增量的调试参数值
    param_debug_velocity_disp.target_vx:set(target_vx)
	param_debug_velocity_disp.target_vy:set(target_vy)
    param_debug_velocity_disp.target_vz:set(target_vz)
	param_debug_velocity_disp.target_head:set(target_head)
	param_debug_velocity_disp.target_rel_head:set(target_rel_head)
    param_debug_velocity_disp.pred_az:set(pred_delta_az_deg)
    param_debug_velocity_disp.pred_el:set(pred_delta_el_deg)
    param_debug_velocity_disp.ref_mode:set(ref_mode_str)
end

-------------------------------------------------------------
-- update_acm_targets 实现：利用完整的 3D 旋转方法来进行转换
-------------------------------------------------------------
local function update_acm_targets()
    local DT = update_time_step  -- 时间步（秒）
    local interp_alpha = 0.8     -- 插值平滑系数
    local MAX_SPEED = math.rad(60) * DT  -- 限制每帧最大角变化（弧度单位）

    -- 首次初始化（复制初始雷达数据到显示变量）
    if not hmd_init_done then
        for i = 1, targetnum do
            local t = rdr_contact_params_h[i]
            if t.time:get() > 0 then
                hmd_display_contacts[i].azimuth   = t.azimuth:get()
                hmd_display_contacts[i].elevation = t.elevation:get()
                hmd_display_contacts[i].range     = t.range:get()
                hmd_display_contacts[i].vel_az    = 0
                hmd_display_contacts[i].vel_el    = 0
            end
        end
        hmd_init_done = true
        return
    end

    -- 获取本机姿态数据（单位：弧度），DCS 中 pitch、roll 均从传感器获得
    local aircraft_pitch = Sensor_Data_Raw.getPitch() or 0
    local aircraft_roll  = Sensor_Data_Raw.getRoll() or 0

    -- 头盔偏移：head_az, head_el 是参数句柄，单位为度，将在后面转换
    local head_az_deg = head_az:get() or 0
    local head_el_deg = head_el:get() or 0
    local hmd_az_rad = math.rad(head_az_deg)
    local hmd_el_rad = math.rad(head_el_deg)

    for i = 1, targetnum do
        local t = rdr_contact_params_h[i]
        local disp = hmd_display_contacts[i]
        local target_valid = t.time:get() > 0.1 and t.range:get() > 100

        if target_valid then
            local radar_az = t.azimuth:get()    -- 原始雷达 azimuth（单位：弧度）
            local radar_el = t.elevation:get()  -- 原始雷达 elevation（单位：弧度）
            local target_rng = t.range:get()
            disp.azimuth   = radar_az
            disp.elevation = radar_el
            disp.range     = target_rng
        --[[if radar_stable_status == 0 then
                -- ACM 模式：利用目标速度预测（同原算法）
                local vx = (t.vx and t.vx:get()) or 0
                local vz = (t.vz and t.vz:get()) or 0
                local predicted_delta_az = (vx / target_rng) * DT
                local predicted_delta_el = (vz / target_rng) * DT
                predicted_delta_az = math.max(-MAX_SPEED, math.min(predicted_delta_az, MAX_SPEED))
                predicted_delta_el = math.max(-MAX_SPEED, math.min(predicted_delta_el, MAX_SPEED))
                disp.azimuth   = disp.azimuth + interp_alpha * ((radar_az + predicted_delta_az) - disp.azimuth)
                disp.elevation = disp.elevation + interp_alpha * ((radar_el + predicted_delta_el) - disp.elevation)
                disp.range     = disp.range + interp_alpha * (target_rng - disp.range)
            end]]
        end

        -- 当前 radar 输出（经过平滑）的最终角度
        local final_az = hmd_display_contacts[i].azimuth
        local final_el = hmd_display_contacts[i].elevation

        -- 下面采用完整的 3D 变换：
    --    local new_az, new_el = compensate_for_attitude(final_az, final_el, aircraft_roll, aircraft_pitch)
        -- 1. 将 (final_az, final_el) 转换为单位向量 v
        local v = {
            math.cos(final_el) * math.cos(final_az),
            math.cos(final_el) * math.sin(final_az),
            math.sin(final_el)
        }

        -- 2. 构造用于扣除机体姿态影响的四元数
        local Q_att = Quat_from_euler(aircraft_roll, aircraft_pitch, 0)

        -- 3. 对向量 v 应用四元数旋转，从而得到新向量 v_new，
        --    这个新向量就是目标在驾驶员视角中去除了机体姿态影响的结果。
        local v_new = Rotate_vector_by_quaternion(v, Q_att)

        -- 4. 将 v_new 转换回球坐标得到新的 azimuth 和 elevation，
        local new_az, new_el = Vector_to_angles(v_new)
        -- new_el = arcsin( v_new.z )，new_az = arctan2(v_new.y, v_new.x)

        -- 5. 最后，再应用驾驶员头盔补偿：这里简单将 head_az 和 head_el 加/减到 new_az 和 new_el 上

		local final_display_az = new_az + hmd_az_rad
		local final_display_el = new_el - hmd_el_rad
		if in_hud then
			final_display_az = new_az
			final_display_el = new_el
		end

        -- 将得到的新 az、el 转换为显示平面坐标。这里依然采用简单的 tan() 投影：
        local x = math.tan(final_display_az)
        local y = math.tan(final_display_el)

        -- 若需要考虑其他标度或校正，可在此进行扩展

        -- 更新显示参数（例如目标框在 HMD 上的位置和距离等）
        local range_km = hmd_display_contacts[i].range / 1000
        local range_nm = hmd_display_contacts[i].range * Meter_TO_NM
        local friendly = t.friendly:get()
        local time = t.time:get()
        local time_scale = t.timeSc:get()
        local id       = t.id:get()
        local nctr     = t.NCTR:get()
        if time > life_time then
            time = life_time
        end

        param_h_HMD_acm_targets[i].x:set(x)
        param_h_HMD_acm_targets[i].y:set(y)
        param_h_HMD_acm_targets[i].range_km:set(range_km)
        param_h_HMD_acm_targets[i].range_nm:set(range_nm)
        param_h_HMD_acm_targets[i].friendly:set(friendly)
        param_h_HMD_acm_targets[i].time:set(time)
        param_h_HMD_acm_targets[i].timeSc:set(time_scale)
        param_h_HMD_acm_targets[i].nctr:set(nctr)
        param_h_HMD_acm_targets[i].id:set(id)
    end
end
-- 连续目标刷新函数
local function update_targets_continuous()
    for i = 1, targetnum do
        local contact = rdr_contact_params_h[i]
        -- 目标距离缩放与显示
        if unit_h:get() == 0 then
            processTargetMetric(contact)
        else
            processTargetImperial(contact)
        end
        -- 目标横向偏移
        local scaledRange = contact.rangeOfScale:get()
        contact.azimuth_range:set(scaledRange * math.tan(contact.azimuth:get()))
        -- 目标地速
        local vx = contact.vx:get()
        local vz = contact.vz:get()
        local target_gs = math.sqrt(vx^2 + vz^2) * MS_TO_KMH
        contact.gs:set(target_gs)
        -- 真航向
        local th_rad = math.atan2(vz, vx)
        if th_rad < 0 then th_rad = th_rad + 2 * math.pi end
        local true_heading_deg = (math.deg(th_rad) + 360) % 360
        -- 航向类型
        local hdg_type = get_param_handle("HDG_TYPE"):get()
        local decl = 6
        local target_disp_heading = true_heading_deg
        if hdg_type == 0 then
            target_disp_heading = (true_heading_deg - decl + 360) % 360
        end
        contact.head:set(target_disp_heading)
        -- 相对航向
        local own_disp_heading = get_param_handle('HDG'):get() % 360
        local relative_heading = (target_disp_heading - own_disp_heading + 360) % 360
        if relative_heading > 180 then
            relative_heading =  relative_heading - 360
        end
        contact.rel_head:set(-relative_heading)
    end
end
function update_scan_result()
    local sum_target = 0  -- 有效目标计数器

    -- 获取航向显示方式，hdg_type: 0 表示磁航向，1 表示真航向
    local hdg_type = get_param_handle("HDG_TYPE"):get()
    -- 磁偏角 decl（单位度），用于将目标真航向转换为磁航向
    local decl = 6  -- 此处可根据实际情况调整

    for i = 1, targetnum do
        local contact = rdr_contact_params_h[i]
        contact.id:set(i)  -- 设置目标 ID
        -- 1. 更新目标剩余存活时间
        local currentTime = contact.time:get() - update_time_step
        if currentTime > life_time then
            currentTime = life_time
        end
    --	if currentTime <= 0 then
    --	    contact.azimuth:set(0)
    --	    contact.elevation:set(0)
    --	    contact.range:set(0)
	--		contact.time:set(0)
    --	end
		--contact.time:set(currentTime)
        -- 2. 统计有效目标 (0 < time < 0.82)
        if currentTime > 0 and currentTime < 0.82 then
            sum_target = sum_target + 1
        end

        -- 3. 对目标距离进行缩放处理
        if unit_h:get() == 0 then
            processTargetMetric(contact)
        else
            processTargetImperial(contact)
        end

        -- 4. 根据目标方位角及缩放后距离计算目标横向偏移（水平距离）
        local scaledRange = contact.rangeOfScale:get()
        contact.azimuth_range:set(scaledRange * math.tan(contact.azimuth:get()))

        -- 5. 计算目标地速（水平速度），单位 km/h
        local vx = contact.vx:get()
        local vz = contact.vz:get()
        local target_gs = math.sqrt(vx^2 + vz^2) * MS_TO_KMH
        contact.gs:set(target_gs)

        -- 6. 计算目标真航向
        local th_rad = math.atan2(vz, vx)
        if th_rad < 0 then
            th_rad = th_rad + 2 * math.pi
        end
        local true_heading_deg = (math.deg(th_rad) + 360) % 360

        -- 7. 根据 hdg_type 确定目标显示航向
        local target_disp_heading = true_heading_deg
        if hdg_type == 0 then
            -- 若要求显示磁航向，则 target 显示航向 = (真航向 - decl + 360) mod 360
            target_disp_heading = (true_heading_deg - decl + 360) % 360
        end
        contact.head:set(target_disp_heading)

        -- 8. 获取本机航向
        -- 使用 get_param_handle('HDG'):get() 直接获得度数（本机已获得正确的0-360航向）
        local own_disp_heading = get_param_handle('HDG'):get()  -- 返回的是度数
        -- 即使直接获得0-360的值，也可以取模归一，但通常该值已经在范围内:
        own_disp_heading = own_disp_heading % 360

        -- 9. 计算目标相对航向
        -- 公式: (target_disp_heading - own_disp_heading + 360) mod 360，取最小角差
        local relative_heading = (target_disp_heading - own_disp_heading + 360) % 360
        if relative_heading > 180 then
            relative_heading =  relative_heading - 360
        end
        contact.rel_head:set(-relative_heading)
    end

    -- 10. 更新全局目标数量
    param_h.RDR_CONTACT_COUNT[2] = sum_target
end
--------------------------------------------------------------------------------
--------------------------------------------------------------------------------
-- 简化的 PickTWSTarget 函数
-- 根据 TDC 的方位和距离，在原始雷达目标数组 rdr_contact_params_h 中搜索匹配目标，
-- 如果匹配误差小于 matchThreshold，则返回该目标的 id 和 nctr 信息
--
-- 【旧逻辑】基于 radar_target_tracing_array 的匹配代码已作为注释保留：
--[[
function PickTWSTarget(tdcAzimuth, tdcRange, matchThreshold)
    local bestTargetId = nil
    local bestTargetNCTR = nil
    local bestMatchDistance = nil

    for i, target in ipairs(radar_target_tracing_array) do
        local deltaRange   = math.abs(target.range - tdcRange)
        local r_avg        = (target.range + tdcRange) / 2
        local deltaAzimuth = math.abs(target.azimuth - tdcAzimuth)
        local azimuthDistance = deltaAzimuth * r_avg
        local matchDistance = math.sqrt(deltaRange^2 + azimuthDistance^2)

        if matchDistance <= matchThreshold then
            if (not bestMatchDistance) or (matchDistance < bestMatchDistance) then
                bestMatchDistance = matchDistance
                bestTargetId    = target.id
                bestTargetNCTR  = target.nctr
            end
        end
    end

    return bestTargetId, bestTargetNCTR
end
]]
--------------------------------------------------------------------------------

function PickTWSTarget(tdcAzimuth, tdcRange, matchThreshold)
    local bestTargetId = nil
    local bestTargetNCTR = nil
    local bestMatchDistance = nil
    matchThreshold = matchThreshold or 500  -- 默认匹配门限为 500 米

    for i = 1, targetnum do
        local contact = rdr_contact_params_h[i]
        -- 只处理有效的目标（例如 time 值大于 0）
        if contact.time:get() > life_time_low then
            local range_val   = contact.range:get()      -- 单位：米
            local azimuth_val = contact.azimuth:get()    -- 单位：弧度

            local deltaRange   = math.abs(range_val - tdcRange)
            local r_avg        = (range_val + tdcRange) / 2
            local deltaAzimuth = math.abs(azimuth_val - tdcAzimuth)
            local azimuthDistance = deltaAzimuth * r_avg  -- 角度差转换为弧长（米）
            local matchDistance = math.sqrt(deltaRange^2 + azimuthDistance^2)

            if matchDistance <= matchThreshold then
                if (not bestMatchDistance) or (matchDistance < bestMatchDistance) then
                    bestMatchDistance = matchDistance
                    bestTargetId    = contact.id:get()     -- 采用 contact 中的 id 参数句柄
                    bestTargetNCTR  = contact.NCTR:get()   -- 采用 contact 中的 nctr 参数句柄
                end
            end
        end
    end

    return bestTargetId, bestTargetNCTR
end

--------------------------------------------------------------------------------
-- 新功能函数：从当前 TDC 锁定目标 (LockTWSTargetFromTDC)
--
-- 说明：
--   1. 调用 TDC 参数同步函数保持数据为最新
--   2. 从更新后的 TDC 参数中提取 TDC 当前的方位和距离（单位分别为弧度和米）
--   3. 调用 PickTWSTarget 函数进行匹配，传入目标匹配门限（ 1000 米）
--   4. 如果匹配到目标，则将该目标的 nctr 信息写入显示用途的句柄 "TARGET_NCTR"
--------------------------------------------------------------------------------
function LockTargetFromTDC()
    -- 同步 TDC 参数：先从 param_h 更新到 Radar，再从 Radar 同步到 param_h
    update_tdc_param_h_to_Radar()
    update_tdc_Radar_to_param_h()

    -- 从更新后的 TDC 参数中提取当前方位和距离（示例取 Radar 中的值）
    local tdcAzimuth = Radar.tdc_azi_h:get()   -- 单位：弧度
    local tdcRange   = Radar.tdc_range_h:get()   -- 单位：米

    -- 设置目标匹配门限（例如 200 米，可根据需要调整）
    local matchThreshold = 1000

    -- 调用 PickTWSTarget 进行匹配，并返回目标 id 与 nctr 信息
    local targetId, targetNCTR = PickTWSTarget(tdcAzimuth, tdcRange, matchThreshold)

    if targetId then
        -- 将匹配到目标的 nctr 信息存入显示模块使用的句柄 "TARGET_NCTR"
        param_h.TARGET_NCTR[2] = targetNCTR
        param_h.TARGET_NCTR[1]:set(param_h.TARGET_NCTR[2])
        --Log_and_print(string.format("Lock target: id = %d, nctr = %s", targetId, tostring(targetNCTR)))
        dispatch_action(nil, 509, 1)
    else
        -- 未匹配到目标则清空句柄
        param_h.TARGET_NCTR[2] = nil
        param_h.TARGET_NCTR[1]:set(param_h.TARGET_NCTR[2])
        --Log_and_print("No matching target found for TDC position.")
    end
end
--------------------------------------------------------------------------------
function post_initialize()

	-- 初始化雷达参数	
	init_rdr_contact_params_h()
	init_HMD_acm_targets()
	setRadarStable(1)
	param_h.RADAR_BRT[2] = 1
	param_debug_switch:set(Render_debug_info)
end

function SetCommand(command,value)
	--Log_and_print(string.format("Radar SetCom:  %i Value: %.8f",command,value))
	---------------------------------------------------------------------
	local sz_az = Radar.sz_azimuth_h:get()
	local sz_az_deg = math.deg(sz_az)
	local sz_el = Radar.sz_elevation_h:get()
	local sz_el_deg = math.deg(sz_el)

	local tdc_az = Radar.tdc_azi_h:get()
	if atkMode_h:get()~=2 then
		if param_h.RADAR_WORK_MODE[2] == 0 then
			if command == Keys.SelecterUp and value == 0.0 then
				if sz_el_deg >=30 then
					Radar.sz_elevation_h:set(math.rad(30))
				else
					Radar.sz_elevation_h:set(math.rad(sz_el_deg + 0.5))
				end
			elseif command == Keys.SelecterDown and value == 0.0 then
				if sz_el_deg <=-30 then
					Radar.sz_elevation_h:set(math.rad(-30))
				else
					Radar.sz_elevation_h:set(math.rad(sz_el_deg - 0.5))
				end
			elseif command == Keys.SelecterLeft and value == 0.0 then
				if sz_az_deg >=40 then
					Radar.sz_azimuth_h:set(math.rad(40))
				else
					Radar.sz_azimuth_h:set(math.rad(sz_az_deg + 0.5))
				end
			elseif command == Keys.SelecterRight and value == 0.0 then
				if sz_az_deg <=-40 then
					Radar.sz_azimuth_h:set(math.rad(-40))
				else
					Radar.sz_azimuth_h:set(math.rad(sz_az_deg - 0.5))
				end
			elseif command == Keys.PlaneRadarCenter then
				param_h.tdc_range_scale[2] = 0
			elseif command == Keys.PlaneRadarUp then
				param_h.tdc_range_scale[2] = param_h.tdc_range_scale[2] + 500
			elseif command == Keys.PlaneRadarDown then
				param_h.tdc_range_scale[2] = param_h.tdc_range_scale[2] - 500
			elseif command == Keys.PlaneRadarLeft then
				tdc_az = tdc_az - 0.01
			elseif command == Keys.PlaneRadarRight then
				tdc_az = tdc_az + 0.01
			elseif command == Keys.PlaneRadarUpLeft then
				tdc_az = tdc_az - 0.01
				param_h.tdc_range_scale[2] = param_h.tdc_range_scale[2] + 500
			elseif command == Keys.PlaneRadarUpRight then
				tdc_az = tdc_az + 0.01
				param_h.tdc_range_scale[2] = param_h.tdc_range_scale[2] + 500
			elseif command == Keys.PlaneRadarDownLeft then
				tdc_az = tdc_az - 0.01
				param_h.tdc_range_scale[2] = param_h.tdc_range_scale[2] - 500
			elseif command == Keys.PlaneRadarDownRight then
				tdc_az = tdc_az + 0.01
				param_h.tdc_range_scale[2] = param_h.tdc_range_scale[2] - 500
			elseif command == Keys.setRadarStable then
				setRadarStable(value)
			elseif command == JoyCmd.PlaneSelecterVertical then
				param_h.tdc_range_scale[2] = param_h.tdc_range_scale[2] - 500*value
			elseif command == JoyCmd.PlaneSelecterHorizontal then
				tdc_az = tdc_az - 0.01*value
			elseif command == JoyCmd.PlaneSelecterVerticalAbs then
				value = Limit(value)
				param_h.tdc_range_scale[2] = value*40000
			elseif command == JoyCmd.PlaneSelecterHorizontalAbs then
				tdc_az = scan_angle*value
			elseif command == JoyCmd.PlaneRadarHorizontal then
				Radar.sz_azimuth_h:set(math.rad(Limit(sz_az_deg + 0.5*value,-40,40)))
			elseif command == JoyCmd.PlaneRadarVertical then
				Radar.sz_elevation_h:set(math.rad(Limit(sz_el_deg + 0.5*value,-30,30)))
			elseif command == JoyCmd.PlaneRadarHorizontalAbs then
				Radar.sz_azimuth_h:set(math.rad(40*value))
			elseif command == JoyCmd.PlaneRadarVerticalAbs then
				Radar.sz_elevation_h:set(math.rad(30*value))
			end
		end
		if command >= Keys.STICK_SENSOR_CONTROL_FWD and command <Keys.STICK_SENSOR_CONTROL_RIGHT then
			keyDnTimer[command].dn = value
		elseif command == Keys.STICK_SENSOR_CONTROL_RIGHT then

			--Log_and_print("切换到BVR模式")
			param_h.RADAR_WORK_MODE[2] = 0
			currentScanAngle = scan_angle
			currentScanHeightAngle = scan_height_angle
			setRadarStable(1)
			acmRadarStatus = 0
			acmTryLockIdx = 0
			acmTryLockTime = 0
            param_debug_acm.target:set(acmTryLockIdx)
            param_debug_acm.status:set(acmRadarStatus)
		end
		if command == Keys.PlaneRadarReset then
			debug_ACM_params_to_screen()
			debug_rdr_contact_params_to_screen()
		--	debug_param_h_HMD_acm_targets_to_screen()
		end
        if command == Keys.Plane_HOTAS_TargetManagementSwitchUp then
            if Radar.mode_h:get() == 1 and param_h.RADAR_WORK_MODE[2] == 0 then
                LockTargetFromTDC()
            elseif Radar.mode_h:get() == 2 and param_h.RADAR_WORK_MODE[2] == 0 then
                dispatch_action(nil, 510, 1) -- 发送 雷达锁定命令
                Radar.mode_h:set(1) -- 切换到 扫描 模式
            else
                dispatch_action(nil, 509, 1) -- 发送 雷达锁定命令
            end
        end
	end
	----------------------------------------------------------------------	
	--设置TDC显示移动的最大最小值
	if param_h.tdc_range_scale[2] > 40000 then
		param_h.tdc_range_scale[2] = 40000 -- 目标指示符 距离
	elseif param_h.tdc_range_scale[2] < 0 then
		param_h.tdc_range_scale[2] = 0
	end
	if tdc_az > math.rad(scan_angle) then
		Radar.tdc_azi_h:set(math.rad(scan_angle)) -- 目标指示符 方向
	elseif tdc_az < -math.rad(scan_angle) then
		Radar.tdc_azi_h:set(-math.rad(scan_angle))
	else
		Radar.tdc_azi_h:set(tdc_az)
	end
    -------------------------------------------------
    update_tdc_param_h_to_Radar()
end

-- 飞机自身状态更新函数
function self_change_update()
end

--local test_param = get_param_handle("RADAR_CONTACT_TEST")
--test_param:set(40000)
--local param_ele
--local param_azm
--local param_range
--local param_time

-- 更新雷达扫描结果的函数
-- 此函数将逐一处理所有雷达目标，更新目标剩余时间、位置、方向及相对航向，
-- 同时统计在当前扫描周期内有效的目标数量，并更新全局目标计数参数。

-- 更新雷达扫描结果的函数
-- 此函数处理所有目标，更新目标的剩余时间、位置、方向及相对航向




--------------------------------------------------------------------------------
--【update 函数】
--------------------------------------------------------------------------------
function update()
    --------------------------------------------------------------------------------
    --【1. 更新头盔数据以及 HUD 判断】
    --------------------------------------------------------------------------------
    local head_az_deg = head_az:get()
    local head_el_deg = head_el:get()
    in_hud = (math.abs(head_az_deg) <= HUD_AZ_DEG) and (math.abs(head_el_deg) <= HUD_EL_DEG)
    param_debug_in_hud:set(in_hud and 1 or 0)
	helmet_in_hud:set(in_hud and 1 or 0)

    local in_radar = (math.abs(head_az_deg) <= scan_angle) and (math.abs(head_el_deg) <= scan_height_angle)
    local not_BST = (param_h.RADAR_WORK_MODE[2] ~= 2 and param_h.RADAR_WORK_MODE[2] ~= 3)

    if in_hud or not_BST then
        head_el_rad = 0
        head_az_rad = 0
    else
        head_el_rad = math.rad(head_el_deg)
        head_az_rad = -math.rad(head_az_deg)
    end
    helmet_in_radar_scan:set(in_radar and 1 or 0)

    -- 每帧更新头部数据作为扫描中心，更新到调试句柄中
    param_debug_acm.center_az:set(math.deg(head_az_rad))
    param_debug_acm.center_el:set(math.deg(head_el_rad))
	param_hsi_acm_zone:set(math.deg(head_az_rad))

    --------------------------------------------------------------------------------
    --【2. 更新雷达扫描及目标显示】
    --------------------------------------------------------------------------------
    update_acm_targets()
    updateRadarScan()
    -- 新增：连续目标刷新，刷新速率与扫描角度正相关
    local scan_interval = math.max(1, math.floor(currentScanAngle / scan_angle * base_interval))
    scan_update_counter = scan_update_counter + 1
    if scan_update_counter >= scan_interval then
        update_targets_continuous()
        scan_update_counter = 0
    end
	-- 新加入：目标聚类、显示更新以及调试数据更新
    updateClustering()

	local radarMode = param_h.RADAR_WORK_MODE[2]
	if param_h.RDR_CONTACT_COUNT[2] > 0 and (acmRadarStatus ~= 2 or Radar.mode_h:get() ~= 3) then
		if radarMode == 1 then
			updateWACQ()
		elseif radarMode == 2 or radarMode == 3 then
			updateBst()
		elseif radarMode == 4 then
			updateVACQ()
		elseif radarMode == 5 then
			updateHACQ()
		end
	end

    radar_scan_pos_alig:set(40000)
    updateTargetBox()
    updateBP5()
    updateSensorHotas()
    Radar.antenna_pos:set(radar_scan_pos_align / scan_angle)

    --------------------------------------------------------------------------------
    --【3. 更新 TDC/STT 及其它参数】
    --------------------------------------------------------------------------------
	--update_tdc_param_h_to_Radar()
    local range_ratio = 40 / maxRange_h:get()
	if unit_h:get() == 0 then
		param_h.RADAR_STT_RANGE_SCALE[2] = Radar.stt_range:get() * range_ratio
	else
		param_h.RADAR_STT_RANGE_SCALE[2] = Radar.stt_range:get() * range_ratio * 1000 / 1852
	end
    --------------------------------------------------------------------------------
	for k, v in pairs(param_h) do
        v[1]:set(v[2])
    end

    --------------------------------------------------------------------------------
    --【4. 更新飞机姿态 debug 数据】
    --------------------------------------------------------------------------------
    local pitch = Sensor_Data_Raw.getPitch() or 0
    local roll  = Sensor_Data_Raw.getRoll() or 0
    param_debug_pitch_deg:set(math.deg(pitch))
    param_debug_roll_deg:set(math.deg(roll))
    param_debug_pitch:set(pitch)
    param_debug_roll:set(roll)

    --------------------------------------------------------------------------------
    --【5. 更新 Radar.mode_h 的调试数据】
    --------------------------------------------------------------------------------
    param_debug_radar_mode:set(Radar.mode_h:get())

    --------------------------------------------------------------------------------
    --【6. 更新 param_debug_acm 的部分数据（状态）】
    --------------------------------------------------------------------------------
    param_debug_acm.status:set(acmRadarStatus)
	update_debug_target_data()
    updateAndCleanTimedElements(radar_result_temp_array)
    updateAndCleanTimedElements(radar_target_tracing_array)
end


need_to_be_closed = false --不关闭该lua