"""Optional Tk frontend. Imported only when explicitly requested."""
import queue
import json
import os
import threading
import time
import tkinter as tk
import customtkinter as ctk
from pathlib import Path
from tkinter import messagebox, ttk
from serial.tools import list_ports
from app import Axes, MavlinkWorker, MOTOR_FUNCTIONS, NEUTRAL_Z, normalize_endpoint

class Joystick(tk.Canvas):
    def __init__(self, master, callback, **kwargs):
        self.size = 276
        self.center_point = self.size / 2
        self.radius = 108
        self.knob_radius = 24
        super().__init__(master, width=self.size, height=self.size, bg="#f5f5f2",
                         highlightthickness=0, bd=0, **kwargs)
        self.callback = callback
        self.value = (0.0, 0.0)
        self.return_job = None
        c, r, k = self.center_point, self.radius, self.knob_radius
        self.create_oval(c-r, c-r, c+r, c+r, fill="#e9e9e5", outline="#d8d8d2", width=2)
        self.create_oval(c-70, c-70, c+70, c+70, outline="#deded8", width=1)
        self.create_line(c, c-r+12, c, c+r-12, fill="#d2d2cc", dash=(3, 6))
        self.create_line(c-r+12, c, c+r-12, c, fill="#d2d2cc", dash=(3, 6))
        self.create_text(c, 13, text="FORWARD", fill="#8a8a84", font=("Helvetica", 8, "bold"))
        self.create_text(c, self.size-13, text="REVERSE", fill="#8a8a84", font=("Helvetica", 8, "bold"))
        self.create_text(25, c, text="TURN\nLEFT", fill="#8a8a84", font=("Helvetica", 8, "bold"))
        self.create_text(self.size-25, c, text="TURN\nRIGHT", fill="#8a8a84", font=("Helvetica", 8, "bold"))
        self.knob = self.create_oval(c-k, c-k, c+k, c+k,
                                     fill="#575c61", outline="#42474b", width=2)
        self.bind("<Button-1>", self._move)
        self.bind("<B1-Motion>", self._move)
        self.bind("<ButtonRelease-1>", self._release)

    def _move(self, event):
        if self.return_job is not None:
            self.after_cancel(self.return_job)
            self.return_job = None
        dx, dy = event.x - self.center_point, event.y - self.center_point
        length = max(1.0, (dx * dx + dy * dy) ** 0.5)
        if length > self.radius:
            dx, dy = dx * self.radius / length, dy * self.radius / length
        self.set_immediate(dx / self.radius, -dy / self.radius)

    def set_immediate(self, x: float, y: float):
        if self.return_job is not None:
            self.after_cancel(self.return_job)
            self.return_job = None
        self.value = (x, y)
        c, k = self.center_point, self.knob_radius
        dx, dy = x * self.radius, -y * self.radius
        self.coords(self.knob, c-k+dx, c-k+dy, c+k+dx, c+k+dy)
        self.callback()

    def _release(self, _event=None):
        start_x, start_y = self.value
        started = time.monotonic()
        duration = 0.40

        def ease_back():
            elapsed = time.monotonic() - started
            progress = min(1.0, elapsed / duration)
            # Smoothstep has zero slope at both ends, avoiding a motor kick.
            eased = progress * progress * (3.0 - 2.0 * progress)
            remaining = 1.0 - eased
            x, y = start_x * remaining, start_y * remaining
            self.value = (x, y)
            c, k = self.center_point, self.knob_radius
            dx, dy = x * self.radius, -y * self.radius
            self.coords(self.knob, c-k+dx, c-k+dy, c+k+dx, c+k+dy)
            self.callback()
            if progress < 1.0:
                self.return_job = self.after(20, ease_back)
            else:
                self.return_job = None

        if self.return_job is not None:
            self.after_cancel(self.return_job)
        ease_back()

    def center(self):
        if self.return_job is not None:
            self.after_cancel(self.return_job)
            self.return_job = None
        self.value = (0.0, 0.0)
        c, k = self.center_point, self.knob_radius
        self.coords(self.knob, c-k, c-k, c+k, c+k)
        self.callback()


class SpringScale(ctk.CTkSlider):
    """A Scale that smoothly returns to zero when the pointer is released."""

    def __init__(self, master, **kwargs):
        self.callback = kwargs.pop("command", None)
        kwargs.pop("showvalue", None)
        kwargs.pop("bg", None)
        kwargs.pop("troughcolor", None)
        kwargs.pop("activebackground", None)
        kwargs.pop("highlightthickness", None)
        kwargs.pop("bd", None)
        kwargs.pop("sliderrelief", None)
        if "orient" in kwargs:
            kwargs["orientation"] = kwargs.pop("orient")
        length = kwargs.pop("length", 200)
        if kwargs.get("orientation", "horizontal") == "vertical":
            kwargs.setdefault("height", length)
            kwargs.setdefault("width", 18)
        else:
            kwargs.setdefault("width", length)
            kwargs.setdefault("height", 18)
        super().__init__(master, command=self._changed, corner_radius=10,
                         button_corner_radius=10, button_length=22,
                         fg_color="#dbe3ee", progress_color="#60a5fa",
                         button_color="#2563eb", button_hover_color="#1d4ed8",
                         **kwargs)
        self.return_job = None
        self.bind("<ButtonPress-1>", self._cancel_return, add="+")
        self.bind("<ButtonRelease-1>", self._released, add="+")

    def _changed(self, value):
        if self.callback:
            self.callback(value)

    def _set_and_notify(self, value):
        self.set(value)
        if self.callback:
            self.callback(value)

    def _cancel_return(self, _event=None):
        if self.return_job is not None:
            self.after_cancel(self.return_job)
            self.return_job = None

    def _released(self, _event=None):
        # Wait until Tk's native Scale binding has applied its final value.
        self.after_idle(self.spring_to_center)

    def set_immediate(self, value):
        self._cancel_return()
        self._set_and_notify(value)

    def spring_to_center(self):
        self._cancel_return()
        start = float(self.get())
        if abs(start) < 0.5:
            self._set_and_notify(0)
            return
        started = time.monotonic()
        duration = 0.40

        def ease_back():
            progress = min(1.0, (time.monotonic() - started) / duration)
            eased = progress * progress * (3.0 - 2.0 * progress)
            self._set_and_notify(start * (1.0 - eased))
            if progress < 1.0:
                self.return_job = self.after(20, ease_back)
            else:
                self._set_and_notify(0)
                self.return_job = None

        ease_back()


class App(tk.Tk):
    def __init__(self):
        super().__init__()
        self.title("SUB Control")
        self.geometry("1040x800")
        self.minsize(920, 720)
        self.events = queue.Queue()
        self.worker = MavlinkWorker(self.events)
        self.keys = set()
        self.keyboard_yaw = 0
        self.keyboard_vertical = 0
        self.keyboard_planar = (0, 0)
        self.servo_functions = {}
        self.motor_directions = {}
        self.battery_config_path = Path(__file__).resolve().parent / "battery_settings.json"
        self.battery_config = {
            "cells": 4,
            "capacity_mah": 5500,
            "c_rating": 35,
            "full_v_per_cell": 4.20,
            "empty_v_per_cell": 3.30,
            "warning_percent": 20,
        }
        self.battery_voltage = None
        self.battery_voltage_smoothed = None
        self._load_battery_settings()
        self._configure_styles()
        self.protocol("WM_DELETE_WINDOW", self.close)
        self._build()
        self.refresh_ports()
        self.bind_all("<KeyPress>", self._key_down)
        self.bind_all("<KeyRelease>", self._key_up)
        self.after(50, self._poll)
        self.after(500, self._link_watchdog)

    def _configure_styles(self):
        self.configure(bg="#e8e9e7")
        style = ttk.Style(self)
        try:
            style.theme_use("clam")
        except tk.TclError:
            pass
        style.configure("TFrame", background="#fbfbf9")
        style.configure("TLabel", background="#fbfbf9", foreground="#202224",
                        font=("Helvetica", 9))
        style.configure("TLabelframe", background="#f4f6f8", bordercolor="#d8dee8",
                        borderwidth=1, relief="solid")
        style.configure("TLabelframe.Label", background="#f4f6f8", foreground="#475569",
                        font=("Segoe UI", 9, "bold"))
        style.configure("TButton", font=("Segoe UI", 9), padding=(10, 7),
                        background="#ffffff", foreground="#1e293b", bordercolor="#cbd5e1")
        style.map("TButton", background=[("active", "#e8eef7")])
        style.configure("Primary.TButton", background="#2563eb", foreground="#ffffff",
                        bordercolor="#2563eb", font=("Segoe UI", 9, "bold"))
        style.map("Primary.TButton", background=[("active", "#1d4ed8")])
        style.configure("Danger.TButton", background="#dc2626", foreground="#ffffff",
                        bordercolor="#dc2626", font=("Segoe UI", 9, "bold"))
        style.map("Danger.TButton", background=[("active", "#b91c1c")])
        style.configure("Title.TLabel", font=("Segoe UI", 18, "bold"), foreground="#0f172a")
        style.configure("Subtitle.TLabel", font=("Segoe UI", 9), foreground="#64748b")
        style.configure("Status.TLabel", font=("Segoe UI", 9, "bold"))
        style.configure("Footer.TLabel", background="#e9eef5", foreground="#475569",
                        font=("Segoe UI", 9))
        style.configure("Treeview", rowheight=28, background="#ffffff", fieldbackground="#ffffff",
                        foreground="#26282a", bordercolor="#e1e1dc", font=("Helvetica", 9))
        style.configure("Treeview.Heading", font=("Helvetica", 9, "bold"), padding=(6, 6),
                        background="#f0f0ed", foreground="#4c4f52")
        style.map("Treeview", background=[("selected", "#dbeafe")],
                  foreground=[("selected", "#1e3a8a")])
        style.configure("Horizontal.TProgressbar", troughcolor="#e2e8f0", background="#22c55e",
                        bordercolor="#e2e8f0", lightcolor="#22c55e", darkcolor="#22c55e")

    def _console_button(self, master, text, command, kind="neutral", width=112):
        colors = {
            "neutral": ("#f4f4f1", "#e9e9e4", "#343739", "#deded8"),
            "primary": ("#dcebd5", "#cde2c4", "#27401f", "#c4dcb9"),
            "danger": ("#f7dfdc", "#f1cfcb", "#7b3029", "#edcec9"),
        }
        color, hover, text_color, border = colors[kind]
        return ctk.CTkButton(
            master, text=text, command=command, width=width, height=34,
            corner_radius=11, fg_color=color, hover_color=hover,
            text_color=text_color, border_color=border, border_width=1,
            font=ctk.CTkFont("Helvetica", 12, "bold" if kind != "neutral" else "normal"),
            bg_color="transparent",
        )

    def _console_combo(self, master, values, width):
        return ctk.CTkComboBox(
            master, values=[str(value) for value in values], width=width, height=34,
            corner_radius=10, border_width=1, border_color="#cbd5e1",
            fg_color="#ffffff", button_color="#e8eef7",
            button_hover_color="#dbeafe", dropdown_fg_color="#ffffff",
            dropdown_hover_color="#dbeafe", text_color="#1e293b",
            bg_color="transparent", state="readonly",
        )

    def _build(self):
        shell = ctk.CTkFrame(self, fg_color="#f8f8f6", corner_radius=24,
                             border_width=1, border_color="#d5d6d3")
        shell.pack(fill="both", expand=True, padx=18, pady=18)
        shell.grid_rowconfigure(0, weight=1)
        shell.grid_columnconfigure(1, weight=1)

        rail = ctk.CTkFrame(shell, width=248, fg_color="#282d30", corner_radius=22)
        rail.grid(row=0, column=0, sticky="nsew")
        rail.grid_propagate(False)
        workspace = ctk.CTkFrame(shell, fg_color="#fbfbf9", corner_radius=22)
        workspace.grid(row=0, column=1, sticky="nsew")

        ctk.CTkLabel(rail, text="SUB CONTROL  /  03", text_color="#f4f4f1",
                     font=ctk.CTkFont("Helvetica", 13, "bold")).pack(
                         anchor="w", padx=24, pady=(24, 0))
        ctk.CTkLabel(rail, text="ArduSub vehicle", text_color="#8f9699",
                     font=ctk.CTkFont("Helvetica", 10)).pack(anchor="w", padx=24, pady=(2, 14))

        vehicle_art = tk.Canvas(rail, width=204, height=250, bg="#282d30",
                                highlightthickness=0, bd=0)
        vehicle_art.pack(padx=20, pady=(5, 4))
        vehicle_art.create_oval(47, 27, 157, 223, fill="#353b3e", outline="#555c60", width=2)
        vehicle_art.create_oval(68, 49, 136, 201, fill="#303538", outline="#444a4e")
        vehicle_art.create_oval(80, 102, 124, 146, fill="#5e666a", outline="#788084", width=2)
        vehicle_art.create_oval(21, 57, 62, 100, fill="#202426", outline="#62696d", width=2)
        vehicle_art.create_oval(142, 57, 183, 100, fill="#202426", outline="#62696d", width=2)
        vehicle_art.create_line(42, 79, 72, 93, fill="#757d81", width=3)
        vehicle_art.create_line(162, 79, 132, 93, fill="#757d81", width=3)
        vehicle_art.create_text(102, 236, text="TOP VIEW  •  3 THRUSTERS",
                                fill="#7e8689", font=("Helvetica", 8, "bold"))

        ctk.CTkLabel(rail, text="Vehicle", text_color="#959c9f",
                     font=ctk.CTkFont("Helvetica", 10)).pack(anchor="w", padx=24, pady=(5, 0))
        self.vehicle_label = ctk.CTkLabel(rail, text="No heartbeat", text_color="#f4f4f1",
                                          font=ctk.CTkFont("Helvetica", 14, "bold"))
        self.vehicle_label.pack(anchor="w", padx=24)

        rail_bottom = ctk.CTkFrame(rail, fg_color="transparent")
        rail_bottom.pack(side="bottom", fill="x", padx=24, pady=22)
        self.battery_summary = ctk.CTkLabel(
            rail_bottom, text="Battery  —", text_color="#f4f4f1",
            font=ctk.CTkFont("Helvetica", 13, "bold"), anchor="w"
        )
        self.battery_summary.pack(fill="x")
        self.battery_meta = ctk.CTkLabel(
            rail_bottom, text="Waiting for voltage", text_color="#92999c",
            font=ctk.CTkFont("Helvetica", 9), anchor="w"
        )
        self.battery_meta.pack(fill="x", pady=(0, 5))
        self.battery_bar = ctk.CTkProgressBar(
            rail_bottom, height=7, corner_radius=4, fg_color="#454b4e",
            progress_color="#c4e46c"
        )
        self.battery_bar.set(0)
        self.battery_bar.pack(fill="x", pady=(0, 18))
        self.link_label = ctk.CTkLabel(
            rail_bottom, text="●  DISCONNECTED", text_color="#f08a82",
            font=ctk.CTkFont("Helvetica", 10, "bold"), anchor="w"
        )
        self.link_label.pack(fill="x")

        header = ctk.CTkFrame(workspace, fg_color="transparent")
        header.pack(fill="x", padx=28, pady=(24, 14))
        title = ctk.CTkFrame(header, fg_color="transparent")
        title.pack(side="left")
        ctk.CTkLabel(title, text="Drive", text_color="#17191a",
                     font=ctk.CTkFont("Helvetica", 24, "bold")).pack(anchor="w")
        ctk.CTkLabel(title, text="Differential steering  •  MANUAL control",
                     text_color="#8a8c8e", font=ctk.CTkFont("Helvetica", 10)).pack(anchor="w")

        top = ctk.CTkFrame(header, fg_color="transparent")
        top.pack(side="right")
        self.port = self._console_combo(top, [], 190)
        self.port.grid(row=0, column=0, padx=4)
        self._console_button(top, "↻", self.refresh_ports, width=38).grid(row=0, column=1, padx=4)
        self.baud = self._console_combo(top, (9600, 19200, 38400, 57600, 115200), 105)
        self.baud.set("57600")
        self.rate = self._console_combo(top, (10, 20), 72)
        self.rate.set("20")
        self.link_settings_button = self._console_button(
            top, "•••", self._toggle_link_settings, width=38
        )
        self.link_settings_button.grid(row=0, column=2, padx=4)
        self.connect_button = self._console_button(
            top, "Connect", self.toggle_connect, kind="primary", width=110
        )
        self.connect_button.grid(row=0, column=3, padx=(4, 0))

        separator = ctk.CTkFrame(workspace, height=1, fg_color="#e7e7e3")
        separator.pack(fill="x", padx=28)

        controls = ctk.CTkFrame(workspace, fg_color="transparent")
        controls.pack(fill="both", expand=True, padx=28, pady=18)
        controls.grid_rowconfigure(0, weight=1)
        controls.grid_columnconfigure(0, weight=1)
        controls.grid_columnconfigure(1, minsize=214)

        drive_card = ctk.CTkFrame(controls, fg_color="#f5f5f2", corner_radius=18)
        drive_card.grid(row=0, column=0, sticky="nsew", padx=(0, 12))
        ctk.CTkLabel(drive_card, text="STEER + THROTTLE", text_color="#66696b",
                     font=ctk.CTkFont("Helvetica", 10, "bold")).pack(pady=(18, 0))
        self.joystick = Joystick(drive_card, self._axes_changed)
        self.joystick.pack(expand=True, pady=(4, 0))
        ctk.CTkLabel(drive_card, text="W / S drive    •    A / D steer",
                     text_color="#969896", font=ctk.CTkFont("Helvetica", 9)).pack(pady=(0, 16))

        heave_card = ctk.CTkFrame(controls, fg_color="#f5f5f2", corner_radius=18)
        heave_card.grid(row=0, column=1, sticky="nsew")
        ctk.CTkLabel(heave_card, text="VERTICAL", text_color="#66696b",
                     font=ctk.CTkFont("Helvetica", 10, "bold")).pack(pady=(18, 2))
        ctk.CTkLabel(heave_card, text="RISE", text_color="#969896",
                     font=ctk.CTkFont("Helvetica", 8, "bold")).pack()
        self.vertical = SpringScale(
            heave_card, from_=100, to=-100, orient="vertical", length=230,
            command=lambda _: self._axes_changed()
        )
        self.vertical.set(0)
        self.vertical.pack(expand=True, pady=8)
        ctk.CTkLabel(heave_card, text="DIVE", text_color="#969896",
                     font=ctk.CTkFont("Helvetica", 8, "bold")).pack()
        ctk.CTkLabel(heave_card, text="R / F  •  springs to neutral",
                     text_color="#969896", font=ctk.CTkFont("Helvetica", 9)).pack(pady=(4, 16))

        self.battery_cells = tk.StringVar(value=str(self.battery_config["cells"]))
        self.battery_capacity = tk.StringVar(value=str(self.battery_config["capacity_mah"]))
        self.battery_c_rating = tk.StringVar(value=str(self.battery_config["c_rating"]))
        self.battery_full_cell = tk.StringVar(value=str(self.battery_config["full_v_per_cell"]))
        self.battery_empty_cell = tk.StringVar(value=str(self.battery_config["empty_v_per_cell"]))
        self.battery_warning = tk.StringVar(value=str(self.battery_config["warning_percent"]))
        fields = (
            ("Cells (S)", self.battery_cells, 5),
            ("Capacity mAh", self.battery_capacity, 8),
            ("C rating", self.battery_c_rating, 5),
            ("Full V/cell", self.battery_full_cell, 6),
            ("Empty V/cell", self.battery_empty_cell, 6),
            ("Warn %", self.battery_warning, 5),
        )
        self.battery_settings_frame = ctk.CTkFrame(workspace, fg_color="#f0f0ed", corner_radius=14)
        for column, (label, variable, width) in enumerate(fields):
            group = ctk.CTkFrame(self.battery_settings_frame, fg_color="transparent")
            group.grid(row=0, column=column, padx=(0, 8), sticky="w")
            ctk.CTkLabel(group, text=label, text_color="#66696b",
                         font=ctk.CTkFont("Helvetica", 9)).pack(anchor="w")
            ctk.CTkEntry(group, textvariable=variable, width=max(62, width * 9), height=30,
                         corner_radius=8, border_color="#d0d0cb").pack(anchor="w")
        self._console_button(self.battery_settings_frame, "Apply", self._apply_battery_settings,
                             kind="primary", width=82).grid(
            row=0, column=6, padx=(8, 0), sticky="sew"
        )
        self.battery_settings_frame.pack_forget()

        actions = ctk.CTkFrame(workspace, fg_color="transparent")
        actions.pack(fill="x", padx=28, pady=(0, 14))
        self._console_button(actions, "MANUAL", self.set_manual, width=94).pack(side="left", padx=3)
        self._console_button(actions, "ARM", self.arm, kind="primary", width=82).pack(side="left", padx=3)
        self._console_button(actions, "DISARM", self.disarm, kind="danger", width=92).pack(side="left", padx=3)
        ctk.CTkLabel(actions, text="Output", text_color="#777a7c",
                     font=ctk.CTkFont("Helvetica", 10)).pack(side="left", padx=(18, 4))
        self.gain = ctk.CTkSlider(
            actions, from_=5, to=50, number_of_steps=45, width=170, height=18,
            command=self._gain_changed, corner_radius=10, button_corner_radius=10,
            button_length=22, fg_color="#dbe3ee", progress_color="#60a5fa",
            button_color="#2563eb", button_hover_color="#1d4ed8",
            bg_color="transparent",
        )
        self.gain.set(20)
        self.gain.pack(side="left")
        self.gain_text = ctk.CTkLabel(actions, text="20%", text_color="#55585a",
                                      font=ctk.CTkFont("Helvetica", 10, "bold"))
        self.gain_text.pack(side="left")
        self._console_button(actions, "Motor setup", self._toggle_motor_setup, width=112).pack(
            side="right", padx=3
        )
        self._console_button(actions, "Battery", self._toggle_battery_settings, width=88).pack(
            side="right", padx=3
        )

        assignments = ctk.CTkFrame(workspace, fg_color="#f0f0ed", corner_radius=14)
        self.assignments_frame = assignments
        self.servo_tree = ttk.Treeview(
            assignments, columns=("output", "function", "reversed"),
            show="headings", height=5, selectmode="browse",
        )
        self.servo_tree.heading("output", text="Output")
        self.servo_tree.heading("function", text="Function")
        self.servo_tree.heading("reversed", text="Motor reversed")
        self.servo_tree.column("output", width=90, anchor="center", stretch=False)
        self.servo_tree.column("function", width=220, anchor="w")
        self.servo_tree.column("reversed", width=90, anchor="center", stretch=False)
        for number in range(1, 17):
            self.servo_tree.insert("", "end", iid=str(number),
                                   values=(f"SERVO{number}", "Waiting…", "Waiting…"))
        self.servo_tree.pack(side="left", fill="x", expand=True)
        scrollbar = ttk.Scrollbar(assignments, orient="vertical", command=self.servo_tree.yview)
        scrollbar.pack(side="left", fill="y")
        self.servo_tree.configure(yscrollcommand=scrollbar.set)

        edit = ctk.CTkFrame(assignments, fg_color="transparent")
        edit.pack(side="right", fill="y")
        ctk.CTkLabel(edit, text="Selected output function", text_color="#55585a").pack(anchor="w")
        motor_choices = ["Unassigned (0)"] + [f"Motor {number}" for number in range(1, 13)]
        self.motor_function = self._console_combo(edit, motor_choices, 165)
        self.motor_function.set("Motor 1")
        self.motor_function.pack(fill="x", pady=(2, 5))
        self._console_button(edit, "Apply function", self.apply_servo_function, width=165).pack(fill="x", pady=2)
        self._console_button(edit, "Reverse motor", self.toggle_servo_reversed, width=165).pack(fill="x", pady=2)
        ctk.CTkLabel(edit, text="Changes require DISARMED", text_color="#a16207").pack(pady=(8, 0))

        log_frame = ctk.CTkFrame(workspace, fg_color="transparent")
        log_frame.pack(fill="x", padx=28, pady=(0, 12))
        self.log_frame = log_frame
        self.log_path_text = tk.StringVar(value="Session log starts when connected")
        ctk.CTkLabel(log_frame, textvariable=self.log_path_text, anchor="w", text_color="#8b8d8f",
                     font=ctk.CTkFont("Helvetica", 9)).pack(side="left", fill="x", expand=True)
        self._console_button(log_frame, "Open logs", self.open_logs_folder, width=96).pack(side="right")

        self.status_text = tk.StringVar(value="Ready. Controls are neutral; vehicle is not connected.")
        ctk.CTkLabel(workspace, textvariable=self.status_text, anchor="w", height=34,
                     corner_radius=12, fg_color="#eeeeea", text_color="#65686a",
                     font=ctk.CTkFont("Helvetica", 9)).pack(fill="x", padx=28, pady=(0, 18))

    def refresh_ports(self):
        ports = [p.device for p in list_ports.comports()]
        for sim in ("tcp:127.0.0.1:5760", "udpin:127.0.0.1:14550"):
            if sim not in ports:
                ports.append(sim)
        self.port.configure(values=ports)
        if ports and self.port.get() not in ports:
            self.port.set(ports[0])

    def _toggle_battery_settings(self):
        if self.battery_settings_frame.winfo_ismapped():
            self.battery_settings_frame.pack_forget()
        else:
            self.battery_settings_frame.pack(
                fill="x", padx=28, pady=(0, 12), before=self.log_frame
            )

    def _toggle_link_settings(self):
        if self.baud.winfo_ismapped():
            self.baud.grid_remove()
            self.rate.grid_remove()
        else:
            self.baud.grid(row=1, column=0, sticky="e", padx=4, pady=(6, 0))
            self.rate.grid(row=1, column=1, sticky="w", padx=4, pady=(6, 0))

    def _toggle_motor_setup(self):
        if self.assignments_frame.winfo_ismapped():
            self.assignments_frame.pack_forget()
        else:
            self.assignments_frame.pack(
                fill="x", padx=28, pady=(0, 12), before=self.log_frame
            )

    def _load_battery_settings(self):
        try:
            saved = json.loads(self.battery_config_path.read_text(encoding="utf-8"))
            for key in self.battery_config:
                if key in saved:
                    self.battery_config[key] = saved[key]
        except (OSError, ValueError, TypeError):
            pass

    def _apply_battery_settings(self):
        try:
            config = {
                "cells": int(self.battery_cells.get()),
                "capacity_mah": int(self.battery_capacity.get()),
                "c_rating": float(self.battery_c_rating.get()),
                "full_v_per_cell": float(self.battery_full_cell.get()),
                "empty_v_per_cell": float(self.battery_empty_cell.get()),
                "warning_percent": int(self.battery_warning.get()),
            }
            if not 1 <= config["cells"] <= 12:
                raise ValueError("cell count must be between 1 and 12")
            if config["capacity_mah"] <= 0 or config["c_rating"] <= 0:
                raise ValueError("capacity and C rating must be positive")
            if not config["full_v_per_cell"] > config["empty_v_per_cell"] > 0:
                raise ValueError("full V/cell must be greater than empty V/cell")
            if not 0 <= config["warning_percent"] <= 100:
                raise ValueError("warning percentage must be between 0 and 100")
            self.battery_config = config
            self.battery_config_path.write_text(
                json.dumps(config, indent=2) + "\n", encoding="utf-8"
            )
            self._update_battery_display()
            self.status_text.set("Battery settings saved")
        except (ValueError, OSError) as exc:
            messagebox.showerror("Invalid battery settings", str(exc))

    def _battery_percent(self, voltage):
        cells = self.battery_config["cells"]
        per_cell = voltage / cells
        empty = self.battery_config["empty_v_per_cell"]
        full = self.battery_config["full_v_per_cell"]
        return max(0.0, min(100.0, (per_cell - empty) * 100.0 / (full - empty)))

    def _update_battery_display(self):
        voltage = self.battery_voltage_smoothed
        if voltage is None:
            self.battery_summary.configure(text="Battery  —", text_color="#f4f4f1")
            self.battery_meta.configure(text="Waiting for voltage")
            self.battery_bar.set(0)
            return
        cells = self.battery_config["cells"]
        percent = self._battery_percent(voltage)
        per_cell = voltage / cells
        warning = percent <= self.battery_config["warning_percent"]
        self.battery_summary.configure(
            text=f"Battery  {percent:.0f}%",
            text_color="#ffaaa2" if warning else "#f4f4f1",
        )
        self.battery_meta.configure(
            text=f"{voltage:.2f} V  •  {per_cell:.2f} V/cell  •  {cells}S"
                 + ("  •  LOW" if warning else "")
        )
        self.battery_bar.configure(progress_color="#f08a82" if warning else "#c4e46c")
        self.battery_bar.set(percent / 100.0)

    def toggle_connect(self):
        if self.worker.thread and self.worker.thread.is_alive():
            self.worker.disconnect()
            self.connect_button.configure(state="disabled")
            return
        if not self.port.get():
            messagebox.showerror("No port", "Select a USB serial COM port.")
            return
        self.worker.rate_hz = int(self.rate.get())
        self.worker.connect(self.port.get(), int(self.baud.get()))
        self.connect_button.configure(text="Disconnect")
        self.status_text.set("Opening serial port…")

    def _axes_changed(self):
        turn, forward = self.joystick.value
        self.worker.set_axes(Axes(forward, 0.0,
                                  float(self.vertical.get()) / 100,
                                  turn))

    def _gain_changed(self, value):
        value = int(float(value))
        self.worker.gain = value / 100
        self.gain_text.configure(text=f"{value}%")

    def _key_down(self, event):
        key = event.keysym.lower()
        if key in "wasdqerf":
            self.keys.add(key)
            self._apply_keys()

    def _key_up(self, event):
        key = event.keysym.lower()
        self.keys.discard(key)
        self._apply_keys()

    def _apply_keys(self):
        turn = (1 if ("d" in self.keys or "e" in self.keys) else 0) - (
            1 if ("a" in self.keys or "q" in self.keys) else 0
        )
        forward = (1 if "w" in self.keys else 0) - (1 if "s" in self.keys else 0)
        vertical = (1 if "r" in self.keys else 0) - (1 if "f" in self.keys else 0)
        planar = (turn, forward)
        if planar != self.keyboard_planar:
            if turn or forward:
                self.joystick.set_immediate(float(turn), float(forward))
            else:
                self.joystick._release()
            self.keyboard_planar = planar
        if vertical != self.keyboard_vertical:
            if vertical:
                self.vertical.set_immediate(vertical * 100)
            else:
                self.vertical.spring_to_center()
            self.keyboard_vertical = vertical
        self._axes_changed()

    def arm(self):
        if not messagebox.askyesno("Arm vehicle", "Propellers removed or vehicle securely restrained?\n\nArm now?"):
            return
        try:
            self.worker.log("OPERATOR", "clicked ARM")
            self.worker.command_arm(True)
            self.status_text.set("Arm command sent; waiting for ACK/heartbeat…")
        except Exception as exc:
            messagebox.showerror("Cannot arm", str(exc))

    def disarm(self):
        try:
            self.worker.log("OPERATOR", "clicked DISARM")
            self.worker.command_arm(False)
            self.status_text.set("Disarm command sent; neutral sent")
        except Exception as exc:
            messagebox.showerror("Cannot disarm", str(exc))

    def set_manual(self):
        try:
            self.worker.log("OPERATOR", "clicked SET MANUAL")
            self.worker.set_manual_mode()
            self.status_text.set("MANUAL mode request sent")
        except Exception as exc:
            messagebox.showerror("Cannot set mode", str(exc))

    def open_logs_folder(self):
        log_dir = Path(__file__).resolve().parent / "logs"
        log_dir.mkdir(exist_ok=True)
        os.startfile(log_dir)

    def _selected_servo(self):
        selection = self.servo_tree.selection()
        if not selection:
            raise RuntimeError("Select a SERVO output in the table first")
        return int(selection[0])

    def apply_servo_function(self):
        try:
            servo = self._selected_servo()
            choice = self.motor_function.get()
            if choice.startswith("Motor "):
                motor = int(choice.split()[1])
                function = 32 + motor if motor <= 8 else 73 + motor
            else:
                function = 0
            description = MOTOR_FUNCTIONS.get(function, "Unassigned")
            if not messagebox.askyesno(
                "Change output function",
                f"Set SERVO{servo}_FUNCTION to {description} ({function})?\n\n"
                "This changes the autopilot configuration.",
            ):
                return
            self.worker.set_parameter(f"SERVO{servo}_FUNCTION", function)
        except Exception as exc:
            messagebox.showerror("Cannot change function", str(exc))

    def toggle_servo_reversed(self):
        try:
            servo = self._selected_servo()
            function = self.servo_functions.get(servo)
            motor = motor_number_for_function(function)
            if motor is None:
                raise RuntimeError(
                    f"SERVO{servo} is not assigned to an ArduSub Motor function"
                )
            current = self.motor_directions.get(motor)
            if current is None:
                raise RuntimeError(f"MOT_{motor}_DIRECTION has not been read yet")
            new_value = -1 if current == 1 else 1
            if not messagebox.askyesno(
                "Reverse motor output",
                f"SERVO{servo} is Motor {motor}.\n"
                f"Set MOT_{motor}_DIRECTION to {new_value}?\n\n"
                "Test direction with propellers removed after changing this.",
            ):
                return
            self.worker.set_parameter(f"MOT_{motor}_DIRECTION", new_value)
        except Exception as exc:
            messagebox.showerror("Cannot reverse output", str(exc))

    def _refresh_servo_row(self, number):
        function = self.servo_functions.get(number)
        motor = motor_number_for_function(function)
        direction = self.motor_directions.get(motor) if motor is not None else None
        if function is None:
            function_text = "Waiting…"
        else:
            function_text = f"{MOTOR_FUNCTIONS.get(function, 'Other / unassigned')} ({function})"
        if motor is None:
            reversed_text = "—"
        elif direction is None:
            reversed_text = "Waiting…"
        else:
            reversed_text = "Yes" if direction == -1 else "No"
        self.servo_tree.item(str(number), values=(f"SERVO{number}", function_text, reversed_text))

    def _poll(self):
        try:
            while True:
                event = self.events.get_nowait()
                kind = event[0]
                if kind == "heartbeat":
                    _, system, autopilot, mode, armed = event[:5]
                    self.vehicle_label.configure(
                        text=f"{mode}  •  {'ARMED' if armed else 'DISARMED'}"
                    )
                    self.link_label.configure(text="●  LINK OK", text_color="#c4e46c")
                elif kind in ("status", "statustext"):
                    self.status_text.set(event[1])
                elif kind == "ack":
                    self.status_text.set(f"Command {event[1]} ACK: {event[2]}")
                elif kind == "servo_param":
                    _, param_id, value = event
                    number = int(param_id[5:param_id.index("_")])
                    self.servo_functions[number] = value
                    self._refresh_servo_row(number)
                elif kind == "motor_direction":
                    _, motor, value = event
                    self.motor_directions[motor] = value
                    for number in range(1, 17):
                        if motor_number_for_function(self.servo_functions.get(number)) == motor:
                            self._refresh_servo_row(number)
                elif kind == "battery":
                    self.battery_voltage = event[1]
                    if self.battery_voltage_smoothed is None:
                        self.battery_voltage_smoothed = self.battery_voltage
                    else:
                        self.battery_voltage_smoothed = (
                            0.75 * self.battery_voltage_smoothed + 0.25 * self.battery_voltage
                        )
                    self._update_battery_display()
                elif kind == "param_saved":
                    self.status_text.set(f"Confirmed: {event[1]}={event[2]}")
                elif kind == "param_missing":
                    self.status_text.set(f"No response for {event[1]}; continuing scan…")
                elif kind == "param_scan_complete":
                    count = sum(value in MOTOR_FUNCTIONS for value in self.servo_functions.values())
                    self.status_text.set(f"Servo scan complete: {count} motor output(s) assigned")
                elif kind == "log_path":
                    self.log_path_text.set(f"Log: {event[1]}")
                elif kind == "error":
                    self.status_text.set(event[1])
                    messagebox.showerror("MAVLink", event[1])
                elif kind == "disconnected":
                    self.connect_button.configure(text="Connect", state="normal")
                    self.link_label.configure(text="●  DISCONNECTED", text_color="#f08a82")
                    self.vehicle_label.configure(text="No heartbeat")
                    self.battery_voltage = None
                    self.battery_voltage_smoothed = None
                    self._update_battery_display()
                    self.servo_functions.clear()
                    self.motor_directions.clear()
                    for number in range(1, 17):
                        self.servo_tree.item(str(number), values=(f"SERVO{number}", "Waiting…", "Waiting…"))
        except queue.Empty:
            pass
        self.after(50, self._poll)

    def _link_watchdog(self):
        if self.worker.target_system and time.monotonic() - self.worker.last_heartbeat > 5.0:
            if time.monotonic() - self.worker.last_rx < 2.0:
                text = "● DATA OK / HEARTBEAT DELAYED"
            else:
                text = "● ALL LINK DATA LOST"
            self.link_label.configure(text=text, text_color="#f08a82")
            self.worker._send_control(force_neutral=True)
        self.after(500, self._link_watchdog)

    def close(self):
        self.worker.disconnect()
        self.after(150, self.destroy)