# ============================================================
# config_intercept.py
# Параметры для модулей перехвата.
# Импортируется в main.py ПОСЛЕ config.py
# ============================================================

# ─────────────────────────────────────────────────────────────
# 1. RANGE ESTIMATION  (range_estimation.py)
# ─────────────────────────────────────────────────────────────
RANGE_ENABLE = True

# Калибровка камеры (нужно измерить для вашего объектива)
# focal_length_px = focal_length_mm * image_width_px / sensor_width_mm
# Для типичной FPV камеры 2.1mm, 1/3" сенсор (4.8mm), 720px ширина:
#   focal_length_px ≈ 2.1 * 720 / 4.8 ≈ 315
CAMERA_FOCAL_LENGTH_PX = 315.0

# Известный размер цели (метры). Типичный FPV-дрон ~0.25-0.30м по диагонали
TARGET_KNOWN_SIZE_M = 0.27

# Минимальный размер bbox (пиксели) для оценки дальности
RANGE_MIN_APPARENT_SIZE_PX = 4.0

# Максимальная правдоподобная дальность (метры)
RANGE_MAX_M = 500.0
RANGE_MIN_M = 0.3

# Сглаживание оценки дальности (EMA alpha, 0=нет сглаживания, 0.9=сильное)
RANGE_SMOOTH_ALPHA = 0.65

# Tau (time-to-contact)
TAU_SMOOTH_ALPHA = 0.50
TAU_MIN_AREA_RATE = 0.5       # минимальная скорость роста площади (px²/s)
TAU_WARN_SEC = 3.0            # предупреждение: < N секунд до контакта
TAU_CRITICAL_SEC = 1.0        # критическая фаза

# ─────────────────────────────────────────────────────────────
# 2. PROPORTIONAL NAVIGATION  (proportional_navigation.py)
# ─────────────────────────────────────────────────────────────
PN_ENABLE = True

# Навигационная константа (N). Обычно 3-5.
# N=3 — базовый PN, N=4-5 — более агрессивный перехват
PN_NAV_GAIN = 4.0

# Использовать Augmented PN (учитывает ускорение цели)
PN_AUGMENTED = True
PN_AUG_GAIN = 0.5                # вес компенсации ускорения цели

# Максимальная команда ускорения (нормализованная, 0..1)
PN_MAX_ACCEL_CMD = 1.0

# Если range недоступен, fallback к screen-space PN
PN_SCREEN_FALLBACK = True

# Сглаживание LOS rate
PN_LOS_RATE_SMOOTH = 0.40

# Минимальная closing velocity для активации PN (м/с)
PN_MIN_CLOSING_VEL = 0.5

# Lead bias (секунды предсказания вперед для упреждения)
PN_LEAD_TIME_SEC = 0.15

# ─────────────────────────────────────────────────────────────
# 3. INTERCEPT FSM  (intercept_fsm.py)
# ─────────────────────────────────────────────────────────────
INTERCEPT_FSM_ENABLE = True

# Пороги перехода между фазами (по дальности, метры)
FSM_ACQUIRE_RANGE_M = 200.0      # SEARCH → ACQUIRE: цель обнаружена
FSM_TRACK_CONFIRM_HITS = 5       # ACQUIRE → TRACK: стабильный трек
FSM_INTERCEPT_RANGE_M = 80.0     # TRACK → INTERCEPT: начинаем атаку
FSM_TERMINAL_TAU_SEC = 1.5       # INTERCEPT → TERMINAL: осталось < N сек
FSM_TERMINAL_RANGE_M = 8.0       # или расстояние < N метров

# Таймауты (кадры)
FSM_ACQUIRE_TIMEOUT = 150        # не подтвердили → SEARCH
FSM_LOST_TIMEOUT = 60            # потеря в TRACK/INTERCEPT → LOST
FSM_LOST_RECOVER_TIMEOUT = 90    # LOST → SEARCH (полный сброс)

# Throttle / скорость сближения
FSM_INTERCEPT_THROTTLE_BIAS = 0.85  # базовый газ в фазе INTERCEPT
FSM_TERMINAL_THROTTLE = 1.0         # максимальный газ в TERMINAL
FSM_TRACK_THROTTLE_BIAS = 0.60      # удержание дистанции в TRACK

# Terminal phase — переход на инерциальную навигацию
FSM_TERMINAL_USE_KLT_ONLY = True     # в TERMINAL отключить YOLO, только KLT
FSM_TERMINAL_LOCK_COMMANDS = False   # заморозить команды в последний момент
FSM_TERMINAL_LOCK_TAU_SEC = 0.3      # за сколько секунд заморозить

# ─────────────────────────────────────────────────────────────
# 4. AUTOPILOT BRIDGE  (autopilot_bridge.py)
# ─────────────────────────────────────────────────────────────
AUTOPILOT_ENABLE = True
AUTOPILOT_BACKEND = "json"           # "json" | "mavlink" | "msp" | "proto_udp"

# JSON export (расширенный вариант текущего guidance_state.json)
AUTOPILOT_JSON_PATH = "autopilot_cmd.json"
AUTOPILOT_JSON_EVERY = 1             # каждый N-й кадр

# MAVLink
MAVLINK_CONNECTION = "udpout:127.0.0.1:14550"
MAVLINK_SYSTEM_ID = 1
MAVLINK_COMPONENT_ID = 191           # MAV_COMP_ID_ONBOARD_COMPUTER
MAVLINK_CMD_RATE_HZ = 30.0

# MSP (Betaflight / INAV)
MSP_PORT = "/dev/ttyUSB0"            # или "COM3" на Windows
MSP_BAUD = 115200
MSP_CMD_RATE_HZ = 50.0

# Испытательный UDP-протокол "нейросеть -> блок"
PROTO_UDP_ENABLE = False
PROTO_UDP_HOST = "127.0.0.1"
PROTO_UDP_PORT = 5005
PROTO_UDP_DESCRIPTOR = 1

# Маппинг команд наведения → управления дроном
# Коэффициенты конвертации steer → RC channels (микросекунды)
AUTOPILOT_ROLL_SCALE = 500.0         # steer_x * scale + 1500
AUTOPILOT_PITCH_SCALE = 500.0        # steer_y * scale + 1500
AUTOPILOT_YAW_SCALE = 300.0
AUTOPILOT_THROTTLE_CENTER = 1500
AUTOPILOT_RC_MIN = 1000
AUTOPILOT_RC_MAX = 2000

# Экстренный стоп (kill switch при потере цели)
AUTOPILOT_FAILSAFE_ENABLE = True
AUTOPILOT_FAILSAFE_MISS_FRAMES = 30  # потеря > N кадров → hover/RTL
AUTOPILOT_FAILSAFE_ACTION = "hover"  # "hover" | "rtl" | "land"

# ─────────────────────────────────────────────────────────────
# 5. IMM FILTER  (imm_filter.py)
# ─────────────────────────────────────────────────────────────
IMM_ENABLE = True

# Процессный шум для каждой модели
IMM_CV_Q_POS = 2.0        # Constant Velocity — позиция
IMM_CV_Q_VEL = 4.0        # Constant Velocity — скорость
IMM_CA_Q_POS = 1.5        # Constant Acceleration — позиция
IMM_CA_Q_VEL = 3.0        # Constant Acceleration — скорость
IMM_CA_Q_ACC = 12.0       # Constant Acceleration — ускорение
IMM_CT_Q_POS = 2.0        # Coordinated Turn — позиция
IMM_CT_Q_VEL = 5.0        # Coordinated Turn — скорость
IMM_CT_Q_OMEGA = 1.0      # Coordinated Turn — угловая скорость

# Начальные вероятности моделей [CV, CA, CT]
IMM_INIT_PROBS = [0.6, 0.25, 0.15]

# Матрица переходов между моделями (Markov)
# [из CV→CV, из CV→CA, из CV→CT]
# [из CA→CV, из CA→CA, из CA→CT]
# [из CT→CV, из CT→CA, из CT→CT]
IMM_TRANSITION_MATRIX = [
    [0.90, 0.07, 0.03],
    [0.10, 0.82, 0.08],
    [0.08, 0.07, 0.85],
]

# Measurement noise
IMM_R_POS = 16.0
IMM_R_SIZE = 25.0

# Заменить стандартный Kalman8D на IMM (если True, main.py использует IMM)
IMM_REPLACE_KALMAN = False  # включить после тестирования

# ─────────────────────────────────────────────────────────────
# 6. DRAW / DEBUG для новых модулей
# ─────────────────────────────────────────────────────────────
DRAW_RANGE_INFO = True
DRAW_INTERCEPT_FSM = True
DRAW_PN_VECTOR = True
DRAW_IMM_PROBS = True
DRAW_INTERCEPT_POINT = True

from runtime_env import apply_intercept_env_overrides

apply_intercept_env_overrides(globals())