# ============================================================
# INTEGRATION_GUIDE.py
# Гайд по интеграции модулей перехвата в main.py
#
# НЕ ЗАПУСКАТЬ — это документация с фрагментами кода.
# Каждый блок показывает, куда именно вставить код в main.py.
# ============================================================

# ─────────────────────────────────────────────────────────────
# ШАГ 1: ИМПОРТЫ (добавить в начало main.py, после существующих)
# ─────────────────────────────────────────────────────────────

"""
import config_intercept                              # новый конфиг
from config_intercept import *
from range_estimation import RangeEstimator
from proportional_navigation import ProportionalNavigator
from intercept_fsm import InterceptFSM, InterceptPhase
from autopilot_bridge import AutopilotBridge
from imm_filter import IMMFilter
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 2: ИНИЦИАЛИЗАЦИЯ (в main(), после создания guidance_ctrl)
# ─────────────────────────────────────────────────────────────

"""
    # --- Intercept modules ---
    range_est = RangeEstimator()
    print(range_est.status_line())

    pn_nav = ProportionalNavigator()
    print(pn_nav.status_line())

    intercept_fsm = InterceptFSM()
    print(intercept_fsm.status_line())

    autopilot = AutopilotBridge()
    autopilot.start()
    print(autopilot.status_line())

    imm = IMMFilter()
    print(imm.status_line())

    # Состояния для нового пайплайна
    range_state = None
    pn_state = None
    intercept_params = None
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 3: KALMAN → IMM (опционально, если IMM_REPLACE_KALMAN=True)
# Заменяет строку:  kf = Kalman8D()
# ─────────────────────────────────────────────────────────────

"""
    if IMM_REPLACE_KALMAN and IMM_ENABLE:
        kf = imm        # IMMFilter имеет тот же API что Kalman8D
    else:
        kf = Kalman8D()
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 4: RANGE ESTIMATION (внутри главного цикла while True,
#         ПОСЛЕ того как locked_box_eff обновлён и guidance_state
#         вычислен, т.е. перед отрисовкой)
# ─────────────────────────────────────────────────────────────

"""
        # --- Range estimation ---
        if confirmed and locked_box_eff is not None:
            range_state = range_est.update(
                locked_box_eff, frame_ts, dt, confirmed=True
            )
        elif pred_box_eff is not None:
            range_state = range_est.update(
                pred_box_eff, frame_ts, dt, confirmed=False
            )
        else:
            range_state = range_est.update(None, frame_ts, dt)

        # При потере цели — сброс
        if miss_streak >= MAX_MISSES:
            range_est.reset()
            range_state = None
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 5: INTERCEPT FSM (после range estimation)
# ─────────────────────────────────────────────────────────────

"""
        # --- Intercept FSM ---
        intercept_params = intercept_fsm.update(
            confirmed=confirmed,
            hit_streak=hit_streak,
            miss_streak=miss_streak,
            range_state=range_state,
            guidance_state=guidance_state,
            frame_id=frame_id,
        )

        # FSM может переопределить det_every
        if intercept_params.get("det_every_override") is not None:
            det_every = min(int(det_every),
                           int(intercept_params["det_every_override"]))

        # FSM может заставить fullscan
        if intercept_params.get("force_fullscan"):
            need_fullscan = True
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 6: PROPORTIONAL NAVIGATION (после FSM, перед autopilot)
# ─────────────────────────────────────────────────────────────

"""
        # --- Proportional Navigation ---
        pn_state = None
        if intercept_params.get("use_pn") and confirmed:
            # Если есть 3D-оценка дальности
            if (range_state is not None
                and range_state.get("range_m") is not None
                and range_state["confidence"] > 0.3):

                # Позиция цели в метрах (упрощённо: проекция из пикселей)
                target_center = box_center(locked_box_eff if locked_box_eff
                                           is not None else pred_box_eff)
                my_center = np.array([ew * 0.5, eh * 0.5], dtype=np.float32)

                # Пиксели → метры (через range и FOV)
                range_m = float(range_state["range_m"])
                px_to_m = range_m / max(1.0, range_est.focal_px)

                target_pos_m = (target_center - my_center) * px_to_m
                my_pos_m = np.zeros(2, dtype=np.float32)

                # Скорости
                vx_px = float(kf.x[4, 0]) if kf.initialized else 0.0
                vy_px = float(kf.x[5, 0]) if kf.initialized else 0.0
                target_vel_m = np.array([vx_px, vy_px], dtype=np.float32) * px_to_m
                my_vel_m = np.zeros(2, dtype=np.float32)  # неизвестна без IMU

                pn_state = pn_nav.update_3d(
                    my_pos_m, my_vel_m,
                    target_pos_m, target_vel_m,
                    closing_vel=range_state.get("closing_vel", 0.0),
                    timestamp=frame_ts,
                )
            else:
                # Fallback: screen-space PN
                target_center = None
                if locked_box_eff is not None:
                    target_center = box_center(locked_box_eff)
                elif pred_box_eff is not None:
                    target_center = box_center(pred_box_eff)

                target_vel_px = None
                if kf.initialized:
                    target_vel_px = np.array(
                        [float(kf.x[4, 0]), float(kf.x[5, 0])],
                        dtype=np.float32,
                    )

                if target_center is not None:
                    pn_state = pn_nav.update_screen(
                        target_center, ew, eh,
                        target_vel_px, frame_ts,
                    )
        else:
            pn_nav.reset()
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 7: AUTOPILOT BRIDGE (после PN)
# ─────────────────────────────────────────────────────────────

"""
        # --- Autopilot bridge ---
        autopilot_cmd = autopilot.update(
            frame_id=frame_id,
            timestamp=frame_ts,
            guidance_state=guidance_state,
            intercept_params=intercept_params,
            range_state=range_state,
            pn_state=pn_state,
            confirmed=confirmed,
            miss_streak=miss_streak,
        )
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 8: ОТРИСОВКА (после существующей отрисовки, перед writer)
# ─────────────────────────────────────────────────────────────

"""
        # --- Intercept overlays ---
        if range_state is not None:
            range_est.draw_overlay(frame_orig, range_state, sx, sy)

        if pn_state is not None:
            pn_nav.draw_overlay(frame_orig, pn_state, sx, sy)

        if intercept_params is not None:
            intercept_fsm.draw_overlay(frame_orig, intercept_params)

        if IMM_ENABLE and IMM_REPLACE_KALMAN:
            imm.draw_overlay(frame_orig)
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 9: ЛОГИРОВАНИЕ (расширить track_logger.log_frame)
# ─────────────────────────────────────────────────────────────

"""
        # Добавить к существующему вызову track_logger.log_frame():
        # (нужно расширить FIELDNAMES в decision_logger.py)

        # Или создать отдельный лог:
        if intercept_params and range_state:
            # Дополнительные метрики для анализа
            pass  # см. decision_logger.py → добавить поля
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 10: CLEANUP (после главного цикла, перед cap.release())
# ─────────────────────────────────────────────────────────────

"""
    autopilot.stop()
    # Остальной cleanup как прежде:
    autogaze_worker.stop()
    yolo_worker.stop()
    ...
"""

# ─────────────────────────────────────────────────────────────
# ШАГ 11: СБРОС ПРИ ПОТЕРЕ ЦЕЛИ
# В блоках где происходит полный reset (miss_streak >= MAX_MISSES
# и другие блоки сброса в main.py), добавить:
# ─────────────────────────────────────────────────────────────

"""
                range_est.reset()
                pn_nav.reset()
                intercept_fsm.reset()
                range_state = None
                pn_state = None
                intercept_params = None
"""

# =============================================================
# ПОРЯДОК ВЫЗОВОВ В КАЖДОМ КАДРЕ (итоговый пайплайн):
# =============================================================
#
# 1. Чтение кадра, get_effective_frame
# 2. Camera motion compensation
# 3. Kalman/IMM predict
# 4. KLT update
# 5. YOLO submit / get
# 6. ByteTrack update
# 7. Target selection (существующая логика)
# 8. Kalman/IMM update (chosen box)
# 9. Guidance update (screen guidance)            ← существующий
# 10. Range estimation update                     ← НОВЫЙ
# 11. Intercept FSM update                        ← НОВЫЙ
# 12. Proportional Navigation update              ← НОВЫЙ
# 13. Autopilot bridge update                     ← НОВЫЙ
# 14. Decision logger
# 15. Draw overlays
# 16. Video writer / display
# =============================================================

# =============================================================
# КАЛИБРОВКА КАМЕРЫ
# =============================================================
#
# Для точной оценки дальности нужно измерить focal_length_px.
#
# Метод 1 (рекомендуемый):
#   1. Поставить объект известного размера (например, 30см линейку)
#      на известном расстоянии (например, 5 метров)
#   2. Сфотографировать, измерить размер в пикселях
#   3. focal_px = (pixel_size * distance) / real_size
#      Пример: линейка 30см = 42px на расстоянии 5м:
#      focal_px = 42 * 5.0 / 0.3 = 700
#
# Метод 2 (из спецификации камеры):
#   focal_px = focal_mm * image_width_px / sensor_width_mm
#   Типичные FPV камеры:
#   - Caddx Ratel 2:  2.1mm lens, 1/1.8" sensor → ~350px
#   - RunCam Phoenix: 2.1mm lens, 1/2" sensor   → ~315px
#   - DJI O3 Air:     2.1mm lens                 → ~420px
#
# Метод 3 (автокалибровка, грубый):
#   Если знаете размер цели (0.27м для FPV дрона) и примерно
#   расстояние при первом захвате, можно подобрать focal_px
#   итеративно.
# =============================================================
