rov_modes.py

"""Sumber kebenaran TUNGGAL untuk pilot mode ArduSub di sisi Python.

Dipisah dari rov_agent.py dengan alasan yang sama seperti rov_axes.py: logika
pemetaan nama mode dan aturan keselamatan per-mode bisa di-unit-test tanpa
pymavlink, socket, atau hardware.

Dua lapis nama yang sengaja dibedakan:
    - Nama GUI  : "manual", "stabilize", "depth_hold", "poshold", "acro" — yang
      dikirim dashboard lewat command JSON {"name": "pilot_mode", "value": ...}.
    - Nama ArduSub : "MANUAL", "STABILIZE", "ALT_HOLD", "ACRO" — yang dipakai
      master.mode_mapping() dan yang dilaporkan balik lewat HEARTBEAT.

Kenapa "stabilize" sekarang alias ALT_HOLD
    Di STABILIZE attitude (roll/pitch) distabilkan, TAPI kedalaman tidak —
    ArduSub hanya menjalankan cascade PID kedalaman (POSZ->VELZ->PSC_ACCZ) di
    ALT_HOLD. Artinya MANUAL_CONTROL.z = 500 di ALT_HOLD berarti "tahan
    kedalaman", sedangkan di STABILIZE cuma "tidak ada dorongan vertikal":
    wahana yang tidak neutrally buoyant tenggelam/naik kecuali pilot menahan
    stick throttle terus-menerus. Untuk operasi kolam dangkal ini tidak pernah
    menjadi pilihan yang berguna, sementara ALT_HOLD sudah memberi attitude
    stabilized DAN depth hold sekaligus. Jadi kedua nama GUI ("stabilize" dan
    "depth_hold") sekarang mengarah ke ALT_HOLD, dan tab STABILIZE dihapus dari
    dashboard. Aliasnya dipertahankan (bukan dihapus) supaya profil joystick
    tersimpan dengan aksi `mode_stabilize` tetap bekerja.

Mode vs depth-set (sengaja TERPISAH)
    ALT_HOLD = "tahan kedalaman tempat wahana berada sekarang", dikerjakan
    sepenuhnya oleh cascade PID ArduSub. POSHOLD = ALT_HOLD + overlay heading
    hold sisi Python. Itu saja esensi keduanya.

    "Depth-set" (menuju sebuah setpoint yang DIREKAM operator lewat tombol SET,
    lalu dinyalakan lewat tombol ON/OFF) adalah fitur TERSENDIRI. Masuk
    ALT_HOLD/POSHOLD tidak menyalakannya, dan menyalakannya tidak memindahkan
    mode. Dulu keduanya menempel: menekan tab Alt Hold langsung memasang
    setpoint tetap 0.30 m, jadi memilih mode = perintah menyelam.

    Yang TETAP menjadi syarat: bias depth-set hanya dikirim saat mode ArduSub
    ada di DEPTH_HOLD_MODES. Itu syarat fisik (bias mengasumsikan ada cascade
    PID kedalaman yang menerimanya), bukan coupling arah sebaliknya.

Tentang ACRO
    Di ACRO tidak ada stabilisasi attitude sama sekali: stik memerintahkan RATE
    (kecepatan sudut), bukan sudut, dan throttle netral juga tidak menahan
    kedalaman. Karena itu ACRO tidak masuk DEPTH_HOLD_MODES: bias depth-hold
    tidak boleh ikut campur saat tidak ada cascade PID kedalaman yang menahan
    wahana.

Kenapa "poshold" menunjuk ALT_HOLD, bukan mode POSHOLD ArduSub
    POSHOLD ArduSub butuh estimasi posisi horizontal yang dipercaya EKF
    (EK3_SRC1_POSXY = 3, yaitu GPS — lihat parameters_ardusub.params). Di bawah
    air tidak ada GPS, dan wahana ini tidak punya DVL, optical flow, maupun
    sumber VISION_POSITION_ESTIMATE. Meminta mode POSHOLD firmware karena itu
    akan ditolak, atau lebih buruk: diterima dengan estimasi posisi yang
    ngawur.

    Yang dipakai sebagai gantinya adalah OVERLAY sisi Python di atas ALT_HOLD
    (lihat apply_heading_hold di rov_agent.py + rov_heading.py): kedalaman
    ditahan cascade PID ArduSub, heading ditahan koreksi P dari sisi Pi.

    KONSEKUENSI YANG HARUS DIKETAHUI OPERATOR: mode ini menahan KEDALAMAN dan
    HEADING, BUKAN posisi x/y. Arus lateral tetap menggeser wahana dan tidak
    ada sensor yang bisa mendeteksinya.

    Karena "poshold" dan "depth_hold" memetakan ke mode ArduSub yang SAMA,
    HEARTBEAT tidak bisa membedakan keduanya. Yang membedakan adalah flag
    poshold_active di agent, yang dipantulkan lewat telemetry (state["poshold"])
    dan dipakai GUI untuk menyorot tab yang benar.
"""

# Nama GUI -> nama mode ArduSub. "stabilize" sengaja menunjuk ALT_HOLD juga —
# lihat catatan di docstring modul.
PILOT_MODE_MAP = {
    "manual": "MANUAL",
    "stabilize": "ALT_HOLD",
    "depth_hold": "ALT_HOLD",
    # Overlay heading-hold sisi Python, BUKAN mode POSHOLD firmware — lihat
    # docstring modul.
    "poshold": "ALT_HOLD",
    "acro": "ACRO",
}

# Nama GUI untuk station-keep. Dipakai agent supaya tidak membandingkan string
# mentah di tengah handler command.
POSHOLD_MODE = "poshold"

# Mode yang menjalankan cascade PID kedalaman dari autopilot, sehingga
# menggeser setpoint kedalaman lewat bias throttle masuk akal.
#
# INI BUKAN saklar depth-set. Berada di ALT_HOLD tidak menyalakan depth-set,
# dan menyalakan depth-set tidak memindahkan mode — lihat depth_bias_engaged().
#
# MANUAL tidak masuk: tidak ada yang menahan kedalaman, bias hanya akan
# mendorong wahana tanpa umpan balik.
# STABILIZE tidak masuk: attitude distabilkan, tapi kedalaman tidak — lihat
# catatan di docstring modul. Mode ini tidak bisa lagi diminta dari GUI, tapi
# wahana masih bisa berada di sana lewat saklar RC / GCS lain.
# ACRO tidak masuk: lihat catatan di docstring modul.
DEPTH_HOLD_MODES = frozenset({"ALT_HOLD"})

# Mode yang memerlukan konfirmasi eksplisit SEBELUM diminta operator. STABILIZE
# tidak ada di sini karena tidak bisa lagi diminta dari GUI — tapi peringatannya
# tetap hidup di MODE_WARNINGS untuk kasus wahana masuk STABILIZE dari luar.
RISKY_MODES = frozenset({"ACRO"})

ACRO_WARNING = (
    "ACRO: tanpa stabilisasi attitude. Stik = rate, dan throttle netral "
    "TIDAK menahan kedalaman. Depth-set tidak akan menahan di mode ini."
)

STABILIZE_WARNING = (
    "STABILIZE: attitude distabilkan, tapi kedalaman TIDAK. Throttle netral "
    "berarti dorongan vertikal nol, bukan tahan kedalaman. Depth-set tidak "
    "akan menahan di mode ini — tahan stick throttle secara manual."
)

# Peta mode -> pesan peringatan operator, dipakai oleh warning_for_mode().
# Sengaja LEBIH LUAS dari RISKY_MODES: STABILIZE tidak bisa diminta dari GUI
# (jadi tidak perlu gerbang konfirmasi), tapi kalau wahana ternyata berada di
# sana peringatannya tetap perlu muncul.
MODE_WARNINGS = {
    "ACRO": ACRO_WARNING,
    "STABILIZE": STABILIZE_WARNING,
}


def resolve_pilot_mode(name):
    """Terjemahkan nama mode dari GUI ke nama mode ArduSub.

    Menerima huruf besar/kecil bebas dan spasi di tepi. Mengembalikan None
    untuk nama yang tidak dikenal (termasuk None / non-string), supaya pemanggil
    bisa menolak perintah alih-alih diam-diam menebak mode.
    """
    if not isinstance(name, str):
        return None
    return PILOT_MODE_MAP.get(name.strip().lower())


def is_poshold_request(name):
    """True kalau nama mode dari GUI meminta overlay station-keep.

    Sengaja terpisah dari resolve_pilot_mode(): keduanya berujung di ALT_HOLD,
    jadi hasil resolve TIDAK cukup untuk tahu apakah overlay harus hidup.
    """
    if not isinstance(name, str):
        return False
    return name.strip().lower() == POSHOLD_MODE


def depth_hold_allowed(mode):
    """True kalau bias depth-set boleh aktif di `mode`.

    Syarat MODE saja — saklar operator diperiksa depth_bias_engaged().

    `mode` adalah nama ArduSub (mis. dari HEARTBEAT atau requested_mode).
    """
    return mode in DEPTH_HOLD_MODES


def depth_bias_engaged(enabled, target, mode, heave, heave_epsilon):
    """True kalau bias throttle depth-set boleh benar-benar dikirim sekarang.

    Semua gerbang depth-set dikumpulkan di satu fungsi murni supaya bisa diuji
    tanpa pymavlink/socket — pemanggilnya (apply_depth_hold_bias di
    rov_agent.py) tinggal membaca state global lalu menyerahkannya ke sini.

    Urutan gerbang sengaja begini:
      - `target is None` (operator belum menekan SET) DIBEDAKAN dari target
        0.0 yang sah. Tanpa itu, "belum di-set" akan berarti "tahan di
        permukaan" dan wahana naik sendiri begitu depth-set di-ON-kan.
      - `enabled` adalah saklar operator. Masuk ALT_HOLD/POSHOLD TIDAK
        menyalakannya: menahan kedalaman firmware adalah esensi mode itu,
        sedangkan menuju setpoint tersimpan adalah fitur terpisah yang
        di-arm sendiri oleh operator.
      - `mode` tetap harus mode depth hold (lihat DEPTH_HOLD_MODES). Ini
        bukan coupling arah sebaliknya, melainkan syarat fisik: kompensasi
        deadzone yang dipakai bias mengasumsikan ada cascade PID kedalaman
        yang menerimanya. Di MANUAL/ACRO bias jadi dorongan open-loop.
      - stik heave yang dipegang selalu menang atas setpoint mana pun.
    """
    if target is None:
        return False
    if not enabled:
        return False
    if not depth_hold_allowed(mode):
        return False
    return abs(heave) <= heave_epsilon


def is_risky_mode(mode):
    """True untuk mode yang perlu peringatan menonjol ke operator."""
    return mode in RISKY_MODES


def warning_for_mode(mode):
    """Pesan peringatan operator untuk `mode`, atau None kalau tidak ada."""
    return MODE_WARNINGS.get(mode)