import json
import queue
import subprocess
import sys
import threading
import time
import traceback
import math
from pathlib import Path
import tkinter as tk
from tkinter import filedialog, messagebox, ttk

import erpc


REPO_ROOT = Path(__file__).resolve().parents[1]
if str(REPO_ROOT) not in sys.path:
    sys.path.insert(0, str(REPO_ROOT))

from file_service import client as file_service_client
from servo_service import client as servo_service_client
from servo_service import common as servo_service_common
from s_curve import SCurve, SCurveError, SCurvePositionPlanner1D

SERVO_CHANNEL_COUNT = 48
DEFAULT_FILE_CHUNK_SIZE = 4096
CONFIG_DIR = REPO_ROOT / "config"
SERVO_CONFIG_YAML_PATH = CONFIG_DIR / "servo_config.yaml"
SERVO_CONFIG_PATH = CONFIG_DIR / "servo_config.json"
YAML_TO_JSON_SCRIPT = CONFIG_DIR / "yaml_to_json.py"
DEFAULT_REMOTE_SERVO_CONFIG_PATH = "config/servo_config.json"
BLINK_TRANSITION_MS = 90
BLINK_STEP_MS = 15
EYE_CONTROL_SPECS = (
    {"title": "Left Upper Lid", "short": "Upper Lid", "default_id": "eye_l_up", "blink": True},
    {"title": "Left Lower Lid", "short": "Lower Lid", "default_id": "eye_l_lower", "blink": True},
    {"title": "Right Upper Lid", "short": "Upper Lid", "default_id": "eye_r_up", "blink": True},
    {"title": "Right Lower Lid", "short": "Lower Lid", "default_id": "eye_r_lower", "blink": True},
    {"title": "Eyes Horizontal", "short": "Horizontal", "default_id": "", "blink": False},
    {"title": "Eyes Vertical", "short": "Vertical", "default_id": "", "blink": False},
)


class FaceServoRpcClient:
    def __init__(self):
        self._transport = None
        self._manager = None
        self._servo_client = None
        self._file_client = None

    @property
    def is_connected(self):
        return self._servo_client is not None and self._file_client is not None

    def connect(self, port, baud):
        self.close()
        self._transport = erpc.transport.SerialTransport(port, baud)
        self._manager = erpc.client.ClientManager(
            self._transport,
            erpc.basic_codec.BasicCodec,
        )
        self._servo_client = servo_service_client.servo_serviceClient(self._manager)
        self._file_client = file_service_client.file_serviceClient(self._manager)

    def close(self):
        transport = self._transport
        self._servo_client = None
        self._file_client = None
        self._manager = None
        self._transport = None
        if transport and hasattr(transport, "close"):
            transport.close()

    def move(self, items):
        self._require_servo_client()
        cmds = [
            servo_service_common.ServoCmd(id=servo_id, angle_rad=float(angle_rad))
            for servo_id, angle_rad in items
        ]
        return self._servo_client.move(cmds)

    def move_j(self, angles_rad):
        self._require_servo_client()
        return self._servo_client.moveJ([float(value) for value in angles_rad])

    def set_constraints(self, servo_id, max_velocity_rad, max_acceleration_rad, max_jerk_rad):
        self._require_servo_client()
        return self._servo_client.setConstraints(
            servo_id,
            float(max_velocity_rad),
            float(max_acceleration_rad),
            float(max_jerk_rad),
        )

    def set_position_gain(self, servo_id, position_gain):
        self._require_servo_client()
        return self._servo_client.setPositionGain(
            servo_id,
            float(position_gain),
        )

    def set_mode(self, mode):
        self._require_servo_client()
        return self._servo_client.setMode(int(mode))

    def set_update_period_ms(self, ms):
        self._require_servo_client()
        return self._servo_client.setUpdatePeriodMs(int(ms))

    def file_write_begin(self, path, total_size):
        self._require_file_client()
        return self._file_client.fileWriteBegin(path, int(total_size))

    def file_write_chunk(self, data):
        self._require_file_client()
        return self._file_client.fileWriteChunk(data)

    def file_write_end(self):
        self._require_file_client()
        return self._file_client.fileWriteEnd()

    def file_write_abort(self):
        self._require_file_client()
        return self._file_client.fileWriteAbort()

    def upload_file(self, local_path, remote_path, chunk_size, progress=None, cancelled=None):
        self._require_file_client()
        file_path = Path(local_path)
        if not file_path.is_file():
            raise FileNotFoundError(f"Local file not found: {file_path}")
        if not remote_path:
            raise ValueError("Remote path is required.")
        if chunk_size <= 0:
            raise ValueError("Chunk size must be > 0.")

        total_size = file_path.stat().st_size
        if total_size > 0xFFFFFFFF:
            raise ValueError("File is too large for uint32 total_size.")

        if callable(cancelled) and cancelled():
            raise InterruptedError("Upload aborted before start.")

        begin_result = self.file_write_begin(remote_path, total_size)
        if begin_result != 0:
            raise RuntimeError(f"fileWriteBegin failed: {begin_result}")

        bytes_sent = 0
        chunk_count = 0
        try:
            with file_path.open("rb") as stream:
                while True:
                    if callable(cancelled) and cancelled():
                        raise InterruptedError("Upload aborted by user.")

                    chunk = stream.read(chunk_size)
                    if not chunk:
                        break

                    result = self.file_write_chunk(chunk)
                    if result != 0:
                        raise RuntimeError(
                            f"fileWriteChunk failed at offset {bytes_sent}: {result}"
                        )

                    bytes_sent += len(chunk)
                    chunk_count += 1
                    if callable(progress):
                        progress(bytes_sent, total_size)

            end_result = self.file_write_end()
            if end_result != 0:
                raise RuntimeError(f"fileWriteEnd failed: {end_result}")
        except Exception:
            try:
                self.file_write_abort()
            except Exception:
                pass
            raise

        return {
            "local_path": str(file_path),
            "remote_path": remote_path,
            "total_size": total_size,
            "bytes_sent": bytes_sent,
            "chunk_count": chunk_count,
        }

    def _require_servo_client(self):
        if not self._servo_client:
            raise RuntimeError("Not connected. Configure serial and click Connect first.")

    def _require_file_client(self):
        if not self._file_client:
            raise RuntimeError("Not connected. Configure serial and click Connect first.")


class FaceServoControlApp:
    def __init__(self, root):
        self.root = root
        self.root.title("Face Servo Console")
        self.root.geometry("1380x880")
        self.root.minsize(1180, 760)
        self.root.protocol("WM_DELETE_WINDOW", self.on_close)

        self.rpc = FaceServoRpcClient()
        self.events = queue.Queue()
        self.servo_configs, self.servo_config_error = self._load_servo_configs()
        self.servo_id_options = list(self.servo_configs.keys())
        default_servo_id = self._get_preferred_servo_id()

        self.status_var = tk.StringVar(value="Disconnected")
        self.port_var = tk.StringVar(value="COM8")
        self.baud_var = tk.StringVar(value="115200")
        self.single_id_var = tk.StringVar(value=default_servo_id)
        self.single_angle_var = tk.DoubleVar(value=0.0)
        self.single_live_drag_var = tk.BooleanVar(value=True)
        self.single_live_rate_hz_var = tk.StringVar(value="50")
        self.rpc_mode_var = tk.StringVar(value="immediate")
        self.update_period_ms_var = tk.StringVar(value="10")
        self.file_local_path_var = tk.StringVar(value=str(SERVO_CONFIG_YAML_PATH))
        self.file_remote_path_var = tk.StringVar(value=DEFAULT_REMOTE_SERVO_CONFIG_PATH)
        self.file_chunk_size_var = tk.StringVar(value=str(DEFAULT_FILE_CHUNK_SIZE))
        self.file_status_var = tk.StringVar(value="Config upload idle")
        self.file_progress_var = tk.StringVar(value=f"Ready: {SERVO_CONFIG_YAML_PATH.name}")
        self.file_progress_percent_var = tk.DoubleVar(value=0.0)
        self.fill_angle_var = tk.DoubleVar(value=0.0)
        self.blink_hold_ms_var = tk.StringVar(value="120")
        self.last_result_var = tk.StringVar(value="Result: idle")
        self.constraint_servo_id_var = tk.StringVar(value=default_servo_id)
        self.scurve_max_velocity_var = tk.StringVar(value="3.0")
        self.scurve_max_acceleration_var = tk.StringVar(value="10.0")
        self.scurve_max_jerk_var = tk.StringVar(value="50.0")
        self.scurve_start_position_var = tk.StringVar(value="0.0")
        self.scurve_end_position_var = tk.StringVar(value="30.0")
        self.scurve_start_velocity_var = tk.StringVar(value="0.0")
        self.scurve_end_velocity_var = tk.StringVar(value="0.0")
        self.scurve_sample_dt_var = tk.StringVar(value="0.01")
        self.scurve_status_var = tk.StringVar(value="SCurve idle")
        self.scurve_summary_var = tk.StringVar(value="No SCurve plan yet.")
        self.scurve_segments_var = tk.StringVar(value="Segments: -")
        self.follow_servo_id_var = tk.StringVar(value=default_servo_id)
        self.position_planner_max_velocity_var = tk.StringVar(value="3.0")
        self.position_planner_max_acceleration_var = tk.StringVar(value="10.0")
        self.position_planner_max_jerk_var = tk.StringVar(value="50.0")
        self.position_planner_gain_var = tk.StringVar(value="1.0")
        self.position_planner_initial_position_var = tk.StringVar(value="0.0")
        self.position_planner_initial_velocity_var = tk.StringVar(value="0.0")
        self.position_planner_initial_acceleration_var = tk.StringVar(value="0.0")
        self.position_planner_target_position_var = tk.StringVar(value="30.0")
        self.position_planner_sample_dt_var = tk.StringVar(value="0.01")
        self.position_planner_duration_var = tk.StringVar(value="3.0")
        self.position_planner_status_var = tk.StringVar(value="Follow planner idle")
        self.position_planner_summary_var = tk.StringVar(value="No follow plan yet.")
        self.position_planner_target_var = tk.StringVar(value="Target: -")

        self.eye_controls = [
            {
                **spec,
                "id_var": tk.StringVar(value=spec["default_id"]),
                "angle_var": tk.DoubleVar(value=0.0),
            }
            for spec in EYE_CONTROL_SPECS
        ]
        self.scurve = None
        self.position_planner = None
        self._single_live_job = None
        self._single_live_pending = False
        self._file_upload_cancel = threading.Event()
        self._file_upload_active = False
        self._blink_active = False
        self._blink_loop_job = None
        self._preview_update_job = None
        self.notebook = None
        self.single_angle_scale = None
        self.tab_frames = {}
        self.scurve_canvases = {}
        self.scurve_plot_series = {
            "position": None,
            "velocity": None,
            "acceleration": None,
            "jerk": None,
        }
        self.position_planner_canvases = {}
        self.position_planner_plot_series = {
            "position": None,
            "velocity": None,
            "acceleration": None,
            "jerk": None,
        }
        self.colors = {
            "app_bg": "#eef4f7",
            "panel_bg": "#ffffff",
            "panel_edge": "#d5e2ea",
            "hero_text": "#163243",
            "body_text": "#315164",
            "muted_text": "#6f8796",
            "input_bg": "#f7fbfd",
            "input_text": "#173446",
            "accent": "#32b3a4",
            "accent_active": "#54c6b8",
            "accent_text": "#ffffff",
            "ghost": "#e6f1f5",
            "ghost_active": "#d7e8ef",
            "ghost_text": "#21475b",
            "status": "#22906d",
            "notebook_tab": "#ddebf1",
            "notebook_tab_active": "#cfe3eb",
            "notebook_tab_selected": "#32b3a4",
            "log_bg": "#f4fafc",
            "log_text": "#224054",
            "plot_bg": "#f6fbfd",
            "plot_grid": "#d7e6ee",
            "plot_axis": "#a9c0cc",
        }

        self._build_style()
        self._build_layout()
        self._initialize_servo_config_ui()
        self._poll_events()

    def _build_style(self):
        c = self.colors
        self.root.configure(bg=c["app_bg"])
        style = ttk.Style()
        style.theme_use("clam")
        style.configure(
            ".",
            background=c["app_bg"],
            foreground=c["body_text"],
            fieldbackground=c["input_bg"],
            bordercolor=c["panel_edge"],
            lightcolor=c["panel_edge"],
            darkcolor=c["panel_edge"],
        )
        style.configure("TFrame", background=c["app_bg"])
        style.configure("Panel.TFrame", background=c["panel_bg"], relief="flat")
        style.configure(
            "TLabel",
            background=c["app_bg"],
            foreground=c["body_text"],
            font=("Segoe UI", 10),
        )
        style.configure(
            "Header.TLabel",
            background=c["app_bg"],
            foreground=c["hero_text"],
            font=("Bahnschrift SemiBold", 22),
        )
        style.configure(
            "Subhead.TLabel",
            background=c["panel_bg"],
            foreground=c["hero_text"],
            font=("Bahnschrift SemiBold", 12),
        )
        style.configure(
            "Muted.TLabel",
            background=c["panel_bg"],
            foreground=c["muted_text"],
            font=("Segoe UI", 9),
        )
        style.configure(
            "TEntry",
            padding=7,
            foreground=c["input_text"],
            fieldbackground=c["input_bg"],
            insertcolor=c["input_text"],
        )
        style.configure(
            "Accent.TButton",
            background=c["accent"],
            foreground=c["accent_text"],
            padding=(14, 8),
            borderwidth=0,
            font=("Bahnschrift SemiBold", 10),
        )
        style.map(
            "Accent.TButton",
            background=[("active", c["accent_active"]), ("disabled", "#b5c6cf")],
            foreground=[("disabled", "#f6fbfd")],
        )
        style.configure(
            "Ghost.TButton",
            background=c["ghost"],
            foreground=c["ghost_text"],
            padding=(14, 8),
            borderwidth=0,
            font=("Segoe UI Semibold", 10),
        )
        style.map("Ghost.TButton", background=[("active", c["ghost_active"])])
        style.configure(
            "Status.TLabel",
            background=c["panel_bg"],
            foreground=c["status"],
            font=("Segoe UI Semibold", 10),
        )
        style.configure(
            "File.Horizontal.TProgressbar",
            background=c["accent"],
            troughcolor=c["ghost"],
            bordercolor=c["panel_edge"],
            lightcolor=c["accent"],
            darkcolor=c["accent"],
            thickness=12,
        )
        style.configure("TNotebook", background=c["app_bg"], borderwidth=0, tabmargins=(0, 0, 0, 0))
        style.configure(
            "TNotebook.Tab",
            background=c["notebook_tab"],
            foreground=c["ghost_text"],
            padding=(16, 8),
            font=("Segoe UI Semibold", 10),
        )
        style.map(
            "TNotebook.Tab",
            background=[("selected", c["notebook_tab_selected"]), ("active", c["notebook_tab_active"])],
            foreground=[("selected", c["accent_text"])],
        )

    def _build_layout(self):
        shell = ttk.Frame(self.root, padding=18)
        shell.pack(fill="both", expand=True)
        shell.columnconfigure(0, weight=1)
        shell.rowconfigure(1, weight=1)

        hero = ttk.Frame(shell)
        hero.grid(row=0, column=0, sticky="ew", pady=(0, 14))
        hero.columnconfigure(0, weight=1)

        ttk.Label(hero, text="Face Servo Console", style="Header.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        ttk.Label(
            hero,
            text="eRPC serial controller for servo_service and file_service",
            style="TLabel",
        ).grid(row=1, column=0, sticky="w", pady=(4, 0))

        main = ttk.Frame(shell)
        main.grid(row=1, column=0, sticky="nsew")
        main.columnconfigure(0, weight=0)
        main.columnconfigure(1, weight=1)
        main.rowconfigure(0, weight=1)

        left = ttk.Frame(main, style="Panel.TFrame", padding=16)
        left.grid(row=0, column=0, sticky="nsw", padx=(0, 14))
        right = ttk.Frame(main, style="Panel.TFrame", padding=16)
        right.grid(row=0, column=1, sticky="nsew")
        right.columnconfigure(0, weight=1)
        right.rowconfigure(0, weight=1)

        self._build_connection_panel(left)
        self._build_motion_panel(left)
        self._build_single_panel(left)
        self._build_file_panel(left)
        self._build_log_panel(left)
        notebook = ttk.Notebook(right)
        notebook.grid(row=0, column=0, sticky="nsew")
        self.notebook = notebook

        servo_tab = ttk.Frame(notebook, style="Panel.TFrame", padding=16)
        servo_tab.columnconfigure(0, weight=1)
        servo_tab.rowconfigure(1, weight=1)
        notebook.add(servo_tab, text="Servo Matrix")

        scurve_tab = ttk.Frame(notebook, style="Panel.TFrame", padding=16)
        scurve_tab.columnconfigure(0, weight=1)
        scurve_tab.rowconfigure(1, weight=1)
        notebook.add(scurve_tab, text="SCurve Planner")

        position_planner_tab = ttk.Frame(notebook, style="Panel.TFrame", padding=16)
        position_planner_tab.columnconfigure(0, weight=1)
        position_planner_tab.rowconfigure(1, weight=1)
        notebook.add(position_planner_tab, text="Follow Planner")
        self.tab_frames = {
            "servo": servo_tab,
            "scurve": scurve_tab,
            "follow": position_planner_tab,
        }

        self._build_all_angles_panel(servo_tab)
        self._build_scurve_panel(scurve_tab)
        self._build_position_planner_panel(position_planner_tab)

    def _build_connection_panel(self, parent):
        panel = ttk.Frame(parent, style="Panel.TFrame")
        panel.pack(fill="x")
        panel.columnconfigure(1, weight=1)

        ttk.Label(panel, text="Connection", style="Subhead.TLabel").grid(
            row=0, column=0, columnspan=2, sticky="w"
        )
        ttk.Label(panel, text="Serial link to the board", style="Muted.TLabel").grid(
            row=1, column=0, columnspan=2, sticky="w", pady=(0, 14)
        )

        ttk.Label(panel, text="Port").grid(row=2, column=0, sticky="w", pady=4)
        ttk.Entry(panel, textvariable=self.port_var, width=18).grid(
            row=2, column=1, sticky="ew", pady=4
        )

        ttk.Label(panel, text="Baud").grid(row=3, column=0, sticky="w", pady=4)
        ttk.Entry(panel, textvariable=self.baud_var, width=18).grid(
            row=3, column=1, sticky="ew", pady=4
        )

        button_row = ttk.Frame(panel, style="Panel.TFrame")
        button_row.grid(row=4, column=0, columnspan=2, sticky="ew", pady=(10, 8))
        button_row.columnconfigure(0, weight=1)
        button_row.columnconfigure(1, weight=1)

        ttk.Button(
            button_row,
            text="Connect",
            style="Accent.TButton",
            command=self.connect_clicked,
        ).grid(row=0, column=0, sticky="ew", padx=(0, 6))
        ttk.Button(
            button_row,
            text="Disconnect",
            style="Ghost.TButton",
            command=self.disconnect_clicked,
        ).grid(row=0, column=1, sticky="ew", padx=(6, 0))

        ttk.Label(panel, textvariable=self.status_var, style="Status.TLabel").grid(
            row=5, column=0, columnspan=2, sticky="w"
        )
        ttk.Label(panel, textvariable=self.last_result_var, style="Muted.TLabel").grid(
            row=6, column=0, columnspan=2, sticky="w", pady=(4, 0)
        )

    def _build_motion_panel(self, parent):
        panel = ttk.Frame(parent, style="Panel.TFrame")
        panel.pack(fill="x", pady=(22, 0))
        panel.columnconfigure(1, weight=1)

        ttk.Label(panel, text="Motion Settings", style="Subhead.TLabel").grid(
            row=0, column=0, columnspan=2, sticky="w"
        )
        ttk.Label(panel, text="Global setMode() and setUpdatePeriodMs()", style="Muted.TLabel").grid(
            row=1, column=0, columnspan=2, sticky="w", pady=(0, 14)
        )

        ttk.Label(panel, text="Mode").grid(row=2, column=0, sticky="w", pady=4)
        mode_row = ttk.Frame(panel, style="Panel.TFrame")
        mode_row.grid(row=2, column=1, sticky="w", pady=4)
        ttk.Radiobutton(
            mode_row,
            text="Immediate",
            value="immediate",
            variable=self.rpc_mode_var,
            command=self._on_motion_mode_changed,
        ).grid(row=0, column=0, sticky="w")
        ttk.Radiobutton(
            mode_row,
            text="SCurve",
            value="scurve",
            variable=self.rpc_mode_var,
            command=self._on_motion_mode_changed,
        ).grid(row=0, column=1, sticky="w", padx=(12, 0))
        ttk.Radiobutton(
            mode_row,
            text="Follow",
            value="follow",
            variable=self.rpc_mode_var,
            command=self._on_motion_mode_changed,
        ).grid(row=0, column=2, sticky="w", padx=(12, 0))

        ttk.Label(panel, text="Period ms").grid(row=3, column=0, sticky="w", pady=4)
        ttk.Entry(panel, textvariable=self.update_period_ms_var).grid(
            row=3, column=1, sticky="ew", pady=4
        )

        button_row = ttk.Frame(panel, style="Panel.TFrame")
        button_row.grid(row=4, column=0, columnspan=2, sticky="ew", pady=(12, 0))
        button_row.columnconfigure(0, weight=1)
        button_row.columnconfigure(1, weight=1)
        ttk.Button(
            button_row,
            text="Apply Settings",
            style="Accent.TButton",
            command=self.send_motion_settings_clicked,
        ).grid(row=0, column=0, sticky="ew", padx=(0, 6))
        ttk.Button(
            button_row,
            text="Send Mode Only",
            style="Ghost.TButton",
            command=self.send_mode_clicked,
        ).grid(row=0, column=1, sticky="ew", padx=(6, 0))

    def _build_single_panel(self, parent):
        panel = ttk.Frame(parent, style="Panel.TFrame")
        panel.pack(fill="x", pady=(22, 0))
        panel.columnconfigure(1, weight=1)

        ttk.Label(panel, text="Single Servo", style="Subhead.TLabel").grid(
            row=0, column=0, columnspan=2, sticky="w"
        )
        ttk.Label(panel, text="Call move([ServoCmd(id, angle_rad)])", style="Muted.TLabel").grid(
            row=1, column=0, columnspan=2, sticky="w", pady=(0, 14)
        )

        ttk.Label(panel, text="Servo ID").grid(row=2, column=0, sticky="w", pady=4)
        single_id_entry = ttk.Combobox(
            panel,
            textvariable=self.single_id_var,
            values=self.servo_id_options,
            state="normal",
        )
        single_id_entry.grid(row=2, column=1, sticky="ew", pady=4)
        single_id_entry.bind("<<ComboboxSelected>>", self._on_single_servo_id_changed)
        single_id_entry.bind("<FocusOut>", self._on_single_servo_id_changed)
        single_id_entry.bind("<Return>", self._on_single_servo_id_changed)

        ttk.Label(panel, text="Angle").grid(row=3, column=0, sticky="w", pady=4)
        angle_row = ttk.Frame(panel, style="Panel.TFrame")
        angle_row.grid(row=3, column=1, sticky="ew", pady=4)
        angle_row.columnconfigure(0, weight=1)
        self.single_angle_scale = ttk.Scale(
            angle_row,
            variable=self.single_angle_var,
            from_=-math.pi,
            to=math.pi,
            orient="horizontal",
            command=self._on_single_scale_changed,
        )
        self.single_angle_scale.grid(row=0, column=0, sticky="ew", padx=(0, 10))
        single_angle_entry = ttk.Entry(angle_row, textvariable=self.single_angle_var, width=8)
        single_angle_entry.grid(row=0, column=1, sticky="e")
        single_angle_entry.bind("<Return>", self._send_single_from_event)
        self.single_angle_scale.bind("<ButtonRelease-1>", self._on_single_scale_released)

        command_row = ttk.Frame(panel, style="Panel.TFrame")
        command_row.grid(row=4, column=0, columnspan=2, sticky="ew", pady=(10, 0))
        command_row.columnconfigure(0, weight=1)
        command_row.columnconfigure(1, weight=0)
        command_row.columnconfigure(2, weight=0)
        ttk.Checkbutton(
            command_row,
            text="Live drag send",
            variable=self.single_live_drag_var,
            command=self._on_single_live_drag_toggled,
        ).grid(row=0, column=0, sticky="e")
        ttk.Entry(command_row, textvariable=self.single_live_rate_hz_var, width=6).grid(
            row=0, column=1, sticky="e", padx=(10, 6)
        )
        ttk.Label(command_row, text="Hz", style="Muted.TLabel").grid(row=0, column=2, sticky="e")

        ttk.Label(panel, text="Angle unit: rad", style="Muted.TLabel").grid(
            row=5, column=0, columnspan=2, sticky="w", pady=(8, 0)
        )

        single_button_row = ttk.Frame(panel, style="Panel.TFrame")
        single_button_row.grid(row=6, column=0, columnspan=2, sticky="ew", pady=(12, 0))
        single_button_row.columnconfigure(0, weight=1)
        ttk.Button(
            single_button_row,
            text="Send move",
            style="Accent.TButton",
            command=self.send_single_clicked,
        ).grid(row=0, column=0, sticky="ew")

    def _build_log_panel(self, parent):
        panel = ttk.Frame(parent, style="Panel.TFrame")
        panel.pack(fill="both", expand=True, pady=(22, 0))
        panel.columnconfigure(0, weight=1)
        panel.rowconfigure(1, weight=1)

        ttk.Label(panel, text="Activity", style="Subhead.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        self.log_widget = tk.Text(
            panel,
            height=12,
            bg=self.colors["log_bg"],
            fg=self.colors["log_text"],
            insertbackground=self.colors["log_text"],
            relief="flat",
            bd=0,
            wrap="word",
            font=("Consolas", 10),
        )
        self.log_widget.grid(row=1, column=0, sticky="nsew", pady=(10, 0))
        self.log_widget.insert("end", "Console ready.\n")
        self.log_widget.configure(state="disabled")

    def _build_file_panel(self, parent):
        panel = ttk.Frame(parent, style="Panel.TFrame")
        panel.pack(fill="x", pady=(22, 0))
        panel.columnconfigure(1, weight=1)

        ttk.Label(panel, text="Servo Config Upload", style="Subhead.TLabel").grid(
            row=0, column=0, columnspan=2, sticky="w"
        )
        ttk.Label(
            panel,
            text="Convert the local YAML config to JSON, then upload it with file_service.",
            style="Muted.TLabel",
        ).grid(row=1, column=0, columnspan=2, sticky="w", pady=(0, 14))

        ttk.Label(panel, text="Local YAML").grid(row=2, column=0, sticky="w", pady=4)
        local_row = ttk.Frame(panel, style="Panel.TFrame")
        local_row.grid(row=2, column=1, sticky="ew", pady=4)
        local_row.columnconfigure(0, weight=1)
        local_entry = ttk.Entry(local_row, textvariable=self.file_local_path_var)
        local_entry.grid(row=0, column=0, sticky="ew", padx=(0, 8))
        local_entry.bind("<Return>", self._send_file_from_event)
        ttk.Button(
            local_row,
            text="Browse",
            style="Ghost.TButton",
            command=self.browse_file_clicked,
        ).grid(row=0, column=1, sticky="e")

        ttk.Label(panel, text="Remote path").grid(row=3, column=0, sticky="w", pady=4)
        remote_entry = ttk.Entry(panel, textvariable=self.file_remote_path_var)
        remote_entry.grid(row=3, column=1, sticky="ew", pady=4)
        remote_entry.bind("<Return>", self._send_file_from_event)

        ttk.Label(panel, text="Chunk bytes").grid(row=4, column=0, sticky="w", pady=4)
        chunk_entry = ttk.Entry(panel, textvariable=self.file_chunk_size_var)
        chunk_entry.grid(row=4, column=1, sticky="ew", pady=4)
        chunk_entry.bind("<Return>", self._send_file_from_event)

        button_row = ttk.Frame(panel, style="Panel.TFrame")
        button_row.grid(row=5, column=0, columnspan=2, sticky="ew", pady=(12, 0))
        button_row.columnconfigure(0, weight=1)
        button_row.columnconfigure(1, weight=1)
        ttk.Button(
            button_row,
            text="Upload Config",
            style="Accent.TButton",
            command=self.send_file_clicked,
        ).grid(row=0, column=0, sticky="ew", padx=(0, 6))
        ttk.Button(
            button_row,
            text="Abort Upload",
            style="Ghost.TButton",
            command=self.abort_file_clicked,
        ).grid(row=0, column=1, sticky="ew", padx=(6, 0))

        ttk.Label(panel, textvariable=self.file_status_var, style="Status.TLabel").grid(
            row=6, column=0, columnspan=2, sticky="w", pady=(8, 0)
        )
        ttk.Progressbar(
            panel,
            style="File.Horizontal.TProgressbar",
            mode="determinate",
            maximum=100.0,
            variable=self.file_progress_percent_var,
        ).grid(row=7, column=0, columnspan=2, sticky="ew", pady=(8, 0))
        ttk.Label(panel, textvariable=self.file_progress_var, style="Muted.TLabel").grid(
            row=8, column=0, columnspan=2, sticky="w", pady=(4, 0)
        )

    def _build_all_angles_panel(self, parent):
        header = ttk.Frame(parent, style="Panel.TFrame")
        header.grid(row=0, column=0, sticky="ew")
        header.columnconfigure(0, weight=1)

        ttk.Label(header, text="Eye Servo Layout", style="Subhead.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        ttk.Label(
            header,
            text=(
                f"{len(EYE_CONTROL_SPECS)} eye controls with configurable string servo IDs; "
                f"send uses move([ServoCmd(id, angle_rad)]). IDs must match firmware names exactly."
            ),
            style="Muted.TLabel",
        ).grid(row=1, column=0, sticky="w", pady=(0, 14))

        controls = ttk.Frame(header, style="Panel.TFrame")
        controls.grid(row=0, column=1, rowspan=2, sticky="e")
        controls.columnconfigure(0, weight=1)
        ttk.Entry(controls, textvariable=self.fill_angle_var, width=10).grid(
            row=0, column=0, sticky="e", padx=(0, 8)
        )
        ttk.Button(
            controls,
            text="Fill Visible",
            style="Ghost.TButton",
            command=self.fill_all_clicked,
        ).grid(row=0, column=1, sticky="e", padx=(0, 8))
        ttk.Button(
            controls,
            text="Zero Visible",
            style="Ghost.TButton",
            command=self.zero_all_clicked,
        ).grid(row=0, column=2, sticky="e", padx=(0, 8))
        ttk.Button(
            controls,
            text="Send move",
            style="Accent.TButton",
            command=self.send_all_clicked,
        ).grid(row=0, column=3, sticky="e")
        ttk.Label(controls, text="Blink wait (ms)", style="Muted.TLabel").grid(
            row=1, column=0, sticky="e", padx=(0, 8), pady=(10, 0)
        )
        blink_hold_entry = ttk.Entry(controls, textvariable=self.blink_hold_ms_var, width=10)
        blink_hold_entry.grid(row=1, column=1, sticky="e", padx=(0, 8), pady=(10, 0))
        blink_hold_entry.bind("<Return>", self._blink_from_event)
        ttk.Button(
            controls,
            text="Start Blink",
            style="Ghost.TButton",
            command=self.blink_clicked,
        ).grid(row=1, column=2, sticky="e", padx=(0, 8), pady=(10, 0))
        ttk.Button(
            controls,
            text="Stop Blink",
            style="Ghost.TButton",
            command=self.stop_blink_clicked,
        ).grid(row=1, column=3, sticky="e", pady=(10, 0))

        body = ttk.Frame(parent, style="Panel.TFrame")
        body.grid(row=1, column=0, sticky="nsew", pady=(8, 0))
        body.columnconfigure(0, weight=1)
        body.columnconfigure(1, weight=0)
        body.columnconfigure(2, weight=1)
        body.rowconfigure(1, weight=1)

        (
            left_upper,
            left_lower,
            right_upper,
            right_lower,
            gaze_horizontal,
            gaze_vertical,
        ) = self.eye_controls

        self._build_eye_slider_card(body, left_upper).grid(
            row=0, column=0, sticky="ew", padx=(0, 10), pady=(0, 10)
        )
        self._build_vertical_gaze_card(body, gaze_vertical).grid(
            row=0, column=1, rowspan=3, sticky="ns", padx=8, pady=(0, 10)
        )
        self._build_eye_slider_card(body, right_upper).grid(
            row=0, column=2, sticky="ew", padx=(10, 0), pady=(0, 10)
        )

        self._build_eye_preview_card(body, "Left Eye").grid(
            row=1, column=0, sticky="n", padx=(0, 10), pady=(0, 10)
        )
        self._build_eye_preview_card(body, "Right Eye").grid(
            row=1, column=2, sticky="n", padx=(10, 0), pady=(0, 10)
        )

        self._build_eye_slider_card(body, left_lower).grid(
            row=2, column=0, sticky="ew", padx=(0, 10), pady=(0, 10)
        )
        self._build_eye_slider_card(body, right_lower).grid(
            row=2, column=2, sticky="ew", padx=(10, 0), pady=(0, 10)
        )

        self._build_horizontal_gaze_card(body, gaze_horizontal).grid(
            row=3, column=0, columnspan=3, sticky="ew", pady=(6, 0)
        )

    def _build_eye_slider_card(self, parent, control):
        card = ttk.Frame(parent, style="Panel.TFrame", padding=12)
        card.columnconfigure(0, weight=1)
        meta = ttk.Frame(card, style="Panel.TFrame")
        meta.grid(row=0, column=0, sticky="ew", pady=(0, 8))
        meta.columnconfigure(0, weight=1)

        ttk.Label(meta, text=control["title"], style="Subhead.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        ttk.Label(meta, text="ID", style="Muted.TLabel").grid(
            row=0, column=1, sticky="e", padx=(12, 6)
        )
        id_combo = ttk.Combobox(
            meta,
            textvariable=control["id_var"],
            values=self.servo_id_options,
            state="normal",
            width=16,
        )
        id_combo.grid(row=0, column=2, sticky="e")
        id_combo.bind("<<ComboboxSelected>>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        id_combo.bind("<FocusOut>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        id_combo.bind("<Return>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        ttk.Label(card, text="Servo angle (rad)", style="Muted.TLabel").grid(
            row=1, column=0, sticky="w", pady=(0, 8)
        )
        control["scale"] = ttk.Scale(
            card,
            variable=control["angle_var"],
            from_=-math.pi,
            to=math.pi,
            orient="horizontal",
        )
        control["scale"].grid(row=2, column=0, sticky="ew")
        ttk.Entry(card, textvariable=control["angle_var"], width=10).grid(
            row=3, column=0, sticky="ew", pady=(8, 0)
        )
        return card

    def _build_vertical_gaze_card(self, parent, control):
        card = ttk.Frame(parent, style="Panel.TFrame", padding=12)
        card.columnconfigure(0, weight=1)
        meta = ttk.Frame(card, style="Panel.TFrame")
        meta.grid(row=0, column=0, sticky="ew")
        meta.columnconfigure(0, weight=1)

        ttk.Label(meta, text=control["title"], style="Subhead.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        ttk.Label(meta, text="ID", style="Muted.TLabel").grid(
            row=0, column=1, sticky="e", padx=(12, 6)
        )
        id_combo = ttk.Combobox(
            meta,
            textvariable=control["id_var"],
            values=self.servo_id_options,
            state="normal",
            width=16,
        )
        id_combo.grid(row=0, column=2, sticky="e")
        id_combo.bind("<<ComboboxSelected>>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        id_combo.bind("<FocusOut>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        id_combo.bind("<Return>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        ttk.Label(card, text="Both eyes", style="Muted.TLabel").grid(
            row=1, column=0, sticky="w", pady=(2, 10)
        )
        ttk.Label(card, text="Up", style="Muted.TLabel").grid(
            row=2, column=0, sticky="n", pady=(0, 6)
        )
        control["scale"] = ttk.Scale(
            card,
            variable=control["angle_var"],
            from_=math.pi,
            to=-math.pi,
            orient="vertical",
            length=220,
        )
        control["scale"].grid(row=3, column=0, sticky="ns")
        ttk.Label(card, text="Down", style="Muted.TLabel").grid(
            row=4, column=0, sticky="s", pady=(6, 0)
        )
        ttk.Entry(card, textvariable=control["angle_var"], width=10).grid(
            row=5, column=0, sticky="ew", pady=(10, 0)
        )
        return card

    def _build_horizontal_gaze_card(self, parent, control):
        card = ttk.Frame(parent, style="Panel.TFrame", padding=12)
        card.columnconfigure(1, weight=1)
        meta = ttk.Frame(card, style="Panel.TFrame")
        meta.grid(row=0, column=0, columnspan=3, sticky="ew")
        meta.columnconfigure(0, weight=1)

        ttk.Label(meta, text=control["title"], style="Subhead.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        ttk.Label(meta, text="ID", style="Muted.TLabel").grid(
            row=0, column=1, sticky="e", padx=(12, 6)
        )
        id_combo = ttk.Combobox(
            meta,
            textvariable=control["id_var"],
            values=self.servo_id_options,
            state="normal",
            width=16,
        )
        id_combo.grid(row=0, column=2, sticky="e")
        id_combo.bind("<<ComboboxSelected>>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        id_combo.bind("<FocusOut>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        id_combo.bind("<Return>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
        ttk.Label(card, text="Both eyes", style="Muted.TLabel").grid(
            row=1, column=0, columnspan=3, sticky="w", pady=(2, 8)
        )
        ttk.Label(card, text="Left", style="Muted.TLabel").grid(
            row=2, column=0, sticky="w", padx=(0, 8)
        )
        control["scale"] = ttk.Scale(
            card,
            variable=control["angle_var"],
            from_=-math.pi,
            to=math.pi,
            orient="horizontal",
        )
        control["scale"].grid(row=2, column=1, sticky="ew")
        ttk.Label(card, text="Right", style="Muted.TLabel").grid(
            row=2, column=2, sticky="e", padx=(8, 0)
        )
        ttk.Entry(card, textvariable=control["angle_var"], width=10).grid(
            row=3, column=0, columnspan=3, sticky="e", pady=(10, 0)
        )
        return card

    def _build_eye_preview_card(self, parent, title):
        card = ttk.Frame(parent, style="Panel.TFrame", padding=12)
        card.columnconfigure(0, weight=1)
        ttk.Label(card, text=title, style="Subhead.TLabel").grid(
            row=0, column=0
        )

        canvas = tk.Canvas(
            card,
            width=260,
            height=170,
            bg=self.colors["input_bg"],
            highlightthickness=1,
            highlightbackground=self.colors["panel_edge"],
            bd=0,
        )
        canvas.grid(row=1, column=0, pady=(10, 0))
        canvas.create_oval(24, 28, 236, 144, fill="#f3f8fb", outline="#aebfca", width=2)
        canvas.create_arc(24, 18, 236, 128, start=0, extent=180, style="arc", outline="#2b3440", width=12)
        canvas.create_arc(24, 44, 236, 154, start=180, extent=180, style="arc", outline="#2b3440", width=12)
        canvas.create_oval(78, 46, 182, 150, fill="#4169a8", outline="#1a2a44", width=2)
        canvas.create_oval(104, 72, 156, 124, fill="#2e4f86", outline="#16253c", width=1)
        canvas.create_oval(119, 87, 141, 109, fill="#dfe8f1", outline="#16253c", width=1)
        return card

    def _build_scurve_panel(self, parent):
        header = ttk.Frame(parent, style="Panel.TFrame")
        header.grid(row=0, column=0, sticky="ew")
        header.columnconfigure(0, weight=1)

        ttk.Label(header, text="SCurve Planner", style="Subhead.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        ttk.Label(
            header,
            text="Configure local SCurve planning and device setConstraints(id, v, a, j).",
            style="Muted.TLabel",
        ).grid(row=1, column=0, sticky="w", pady=(0, 14))

        controls = ttk.Frame(header, style="Panel.TFrame")
        controls.grid(row=2, column=0, sticky="ew")
        for col in range(4):
            controls.columnconfigure(col, weight=1)

        entries = [
            ("Servo ID", self.constraint_servo_id_var),
            ("Max velocity", self.scurve_max_velocity_var),
            ("Max accel", self.scurve_max_acceleration_var),
            ("Max jerk", self.scurve_max_jerk_var),
            ("Sample dt", self.scurve_sample_dt_var),
            ("Start pos", self.scurve_start_position_var),
            ("End pos", self.scurve_end_position_var),
            ("Start vel", self.scurve_start_velocity_var),
            ("End vel", self.scurve_end_velocity_var),
        ]
        for index, (label_text, variable) in enumerate(entries):
            row = (index // 4) * 2
            col = index % 4
            ttk.Label(controls, text=label_text).grid(row=row, column=col, sticky="w", padx=6, pady=(0, 4))
            if label_text == "Servo ID":
                entry = ttk.Combobox(
                    controls,
                    textvariable=variable,
                    values=self.servo_id_options,
                    state="normal",
                )
                entry.bind("<<ComboboxSelected>>", self._on_constraint_servo_id_changed)
                entry.bind("<FocusOut>", self._on_constraint_servo_id_changed)
                entry.bind("<Return>", self._on_constraint_servo_id_changed)
            else:
                entry = ttk.Entry(controls, textvariable=variable)
                entry.bind("<Return>", self._plan_scurve_from_event)
            entry.grid(row=row + 1, column=col, sticky="ew", padx=6, pady=(0, 10))

        button_row = ttk.Frame(header, style="Panel.TFrame")
        button_row.grid(row=3, column=0, sticky="ew", pady=(4, 0))
        button_row.columnconfigure(0, weight=0)
        button_row.columnconfigure(1, weight=0)
        button_row.columnconfigure(2, weight=1)

        ttk.Button(
            button_row,
            text="Plan Curve",
            style="Accent.TButton",
            command=self.plan_scurve_clicked,
        ).grid(row=0, column=0, sticky="w")
        ttk.Button(
            button_row,
            text="Send Constraints",
            style="Ghost.TButton",
            command=self.send_scurve_constraints_clicked,
        ).grid(row=0, column=1, sticky="w", padx=(10, 0))
        ttk.Label(button_row, textvariable=self.scurve_status_var, style="Status.TLabel").grid(
            row=0, column=2, sticky="w", padx=(14, 0)
        )

        ttk.Label(header, textvariable=self.scurve_summary_var, style="Muted.TLabel").grid(
            row=4, column=0, sticky="w", pady=(10, 0)
        )
        ttk.Label(header, textvariable=self.scurve_segments_var, style="Muted.TLabel").grid(
            row=5, column=0, sticky="w", pady=(4, 0)
        )

        plot_area = ttk.Frame(parent, style="Panel.TFrame")
        plot_area.grid(row=1, column=0, sticky="nsew", pady=(16, 0))
        plot_area.columnconfigure(0, weight=1)
        plot_area.rowconfigure(0, weight=1)
        plot_area.rowconfigure(1, weight=1)
        plot_area.rowconfigure(2, weight=1)
        plot_area.rowconfigure(3, weight=1)

        plot_specs = [
            ("position", "Position", "#7ce0a0"),
            ("velocity", "Velocity", "#65c7ff"),
            ("acceleration", "Acceleration", "#ffd166"),
            ("jerk", "Jerk", "#ff8c7e"),
        ]
        for row_index, (key, title, color) in enumerate(plot_specs):
            card = ttk.Frame(plot_area, style="Panel.TFrame", padding=10)
            card.grid(row=row_index, column=0, sticky="nsew", pady=(0, 10) if row_index < len(plot_specs) - 1 else 0)
            card.columnconfigure(0, weight=1)
            card.rowconfigure(1, weight=1)

            ttk.Label(card, text=title, style="Subhead.TLabel").grid(row=0, column=0, sticky="w")
            ttk.Label(card, text="Time on X axis", style="Muted.TLabel").grid(
                row=0, column=1, sticky="e"
            )
            canvas = tk.Canvas(
                card,
                height=180,
                bg=self.colors["plot_bg"],
                highlightthickness=0,
                bd=0,
            )
            canvas.grid(row=1, column=0, columnspan=2, sticky="nsew", pady=(10, 0))
            canvas.bind("<Configure>", lambda _event, metric=key: self._redraw_curve_plot(metric))
            self.scurve_canvases[key] = canvas
            self.scurve_plot_series[key] = {
                "title": title,
                "color": color,
                "times": [],
                "values": [],
            }

    def _build_position_planner_panel(self, parent):
        header = ttk.Frame(parent, style="Panel.TFrame")
        header.grid(row=0, column=0, sticky="ew")
        header.columnconfigure(0, weight=1)

        ttk.Label(header, text="Follow Planner", style="Subhead.TLabel").grid(
            row=0, column=0, sticky="w"
        )
        ttk.Label(
            header,
            text="Preview SCurvePositionPlanner1D follow behavior and optionally send follow constraints and gain.",
            style="Muted.TLabel",
        ).grid(row=1, column=0, sticky="w", pady=(0, 14))

        controls = ttk.Frame(header, style="Panel.TFrame")
        controls.grid(row=2, column=0, sticky="ew")
        for col in range(5):
            controls.columnconfigure(col, weight=1)

        entries = [
            ("Servo ID", self.follow_servo_id_var),
            ("Max velocity", self.position_planner_max_velocity_var),
            ("Max accel", self.position_planner_max_acceleration_var),
            ("Max jerk", self.position_planner_max_jerk_var),
            ("Pos gain", self.position_planner_gain_var),
            ("Initial pos", self.position_planner_initial_position_var),
            ("Initial vel", self.position_planner_initial_velocity_var),
            ("Initial accel", self.position_planner_initial_acceleration_var),
            ("Target pos", self.position_planner_target_position_var),
            ("Sample dt", self.position_planner_sample_dt_var),
            ("Duration", self.position_planner_duration_var),
        ]
        for index, (label_text, variable) in enumerate(entries):
            row = (index // 5) * 2
            col = index % 5
            ttk.Label(controls, text=label_text).grid(row=row, column=col, sticky="w", padx=6, pady=(0, 4))
            if label_text == "Servo ID":
                entry = ttk.Combobox(
                    controls,
                    textvariable=variable,
                    values=self.servo_id_options,
                    state="normal",
                )
                entry.bind("<<ComboboxSelected>>", self._on_follow_servo_id_changed)
                entry.bind("<FocusOut>", self._on_follow_servo_id_changed)
                entry.bind("<Return>", self._on_follow_servo_id_changed)
            else:
                entry = ttk.Entry(controls, textvariable=variable)
                entry.bind("<Return>", self._plan_position_planner_from_event)
            entry.grid(row=row + 1, column=col, sticky="ew", padx=6, pady=(0, 10))

        button_row = ttk.Frame(header, style="Panel.TFrame")
        button_row.grid(row=3, column=0, sticky="ew", pady=(4, 0))
        button_row.columnconfigure(0, weight=0)
        button_row.columnconfigure(1, weight=0)
        button_row.columnconfigure(2, weight=0)
        button_row.columnconfigure(3, weight=1)

        ttk.Button(
            button_row,
            text="Plan Follow Curve",
            style="Accent.TButton",
            command=self.plan_position_planner_clicked,
        ).grid(row=0, column=0, sticky="w")
        ttk.Button(
            button_row,
            text="Send Constraints",
            style="Ghost.TButton",
            command=self.send_follow_constraints_clicked,
        ).grid(row=0, column=1, sticky="w", padx=(10, 0))
        ttk.Button(
            button_row,
            text="Send Gain",
            style="Ghost.TButton",
            command=self.send_follow_position_gain_clicked,
        ).grid(row=0, column=2, sticky="w", padx=(10, 0))
        ttk.Label(button_row, textvariable=self.position_planner_status_var, style="Status.TLabel").grid(
            row=0, column=3, sticky="w", padx=(14, 0)
        )

        ttk.Label(header, textvariable=self.position_planner_summary_var, style="Muted.TLabel").grid(
            row=4, column=0, sticky="w", pady=(10, 0)
        )
        ttk.Label(header, textvariable=self.position_planner_target_var, style="Muted.TLabel").grid(
            row=5, column=0, sticky="w", pady=(4, 0)
        )

        plot_area = ttk.Frame(parent, style="Panel.TFrame")
        plot_area.grid(row=1, column=0, sticky="nsew", pady=(16, 0))
        plot_area.columnconfigure(0, weight=1)
        plot_area.rowconfigure(0, weight=1)
        plot_area.rowconfigure(1, weight=1)
        plot_area.rowconfigure(2, weight=1)
        plot_area.rowconfigure(3, weight=1)

        plot_specs = [
            ("position", "Position", "#6abf69"),
            ("velocity", "Velocity", "#4da3ff"),
            ("acceleration", "Acceleration", "#f2b43c"),
            ("jerk", "Jerk", "#ef6c73"),
        ]
        for row_index, (key, title, color) in enumerate(plot_specs):
            card = ttk.Frame(plot_area, style="Panel.TFrame", padding=10)
            card.grid(row=row_index, column=0, sticky="nsew", pady=(0, 10) if row_index < len(plot_specs) - 1 else 0)
            card.columnconfigure(0, weight=1)
            card.rowconfigure(1, weight=1)

            ttk.Label(card, text=title, style="Subhead.TLabel").grid(row=0, column=0, sticky="w")
            ttk.Label(card, text="Time on X axis", style="Muted.TLabel").grid(
                row=0, column=1, sticky="e"
            )
            canvas = tk.Canvas(
                card,
                height=180,
                bg=self.colors["plot_bg"],
                highlightthickness=0,
                bd=0,
            )
            canvas.grid(row=1, column=0, columnspan=2, sticky="nsew", pady=(10, 0))
            canvas.bind("<Configure>", lambda _event, metric=key: self._redraw_position_planner_plot(metric))
            self.position_planner_canvases[key] = canvas
            self.position_planner_plot_series[key] = {
                "title": title,
                "color": color,
                "times": [],
                "values": [],
            }

    def _load_servo_configs(self):
        try:
            raw = json.loads(SERVO_CONFIG_PATH.read_text(encoding="utf-8"))
        except Exception as exc:
            return {}, str(exc)

        configs = {}
        for item in raw.get("servos", []):
            servo_id = str(item.get("id", "")).strip()
            if servo_id:
                configs[servo_id] = item
        return configs, None

    def _get_preferred_servo_id(self):
        if "eye_l_up" in self.servo_configs:
            return "eye_l_up"
        if self.servo_id_options:
            return self.servo_id_options[0]
        return ""

    def _initialize_servo_config_ui(self):
        self._apply_single_servo_config(use_home=True)
        self._sync_planner_inputs_from_single()
        self._apply_constraint_servo_config()
        self._apply_follow_servo_config()
        for control in self.eye_controls:
            self._apply_eye_control_config(control, use_home=self._should_use_home_for_eye_control(control))

        if self.servo_config_error:
            self._append_log(f"Servo config load failed: {self.servo_config_error}")
            return
        self._append_log(
            f"Loaded {len(self.servo_configs)} servo configs from {SERVO_CONFIG_PATH}"
        )

    def _get_servo_config(self, servo_id):
        if not servo_id:
            return None
        return self.servo_configs.get(servo_id.strip())

    def _get_servo_limits(self, servo_config):
        if not servo_config:
            return -math.pi, math.pi
        min_angle = float(servo_config.get("limit_min_angle_rad", -math.pi))
        max_angle = float(servo_config.get("limit_max_angle_rad", math.pi))
        if min_angle > max_angle:
            min_angle, max_angle = max_angle, min_angle
        return min_angle, max_angle

    def _apply_scale_limits(self, scale, variable, min_angle, max_angle, *, vertical=False, home_angle=None, use_home=False):
        if scale is not None:
            if vertical:
                scale.configure(from_=max_angle, to=min_angle)
            else:
                scale.configure(from_=min_angle, to=max_angle)

        value = float(variable.get())
        if use_home and home_angle is not None:
            value = float(home_angle)
        value = max(min_angle, min(max_angle, value))
        variable.set(value)

    def _apply_single_servo_config(self, use_home=False):
        servo_config = self._get_servo_config(self.single_id_var.get().strip())
        min_angle, max_angle = self._get_servo_limits(servo_config)
        home_angle = servo_config.get("home_angle_rad") if servo_config else None
        self._apply_scale_limits(
            self.single_angle_scale,
            self.single_angle_var,
            min_angle,
            max_angle,
            home_angle=home_angle,
            use_home=use_home,
        )

    def _apply_constraint_servo_config(self):
        servo_config = self._get_servo_config(self.constraint_servo_id_var.get().strip())
        control = servo_config.get("control", {}) if servo_config else {}
        if not control:
            return
        self.scurve_max_velocity_var.set(str(control.get("max_velocity_rad", self.scurve_max_velocity_var.get())))
        self.scurve_max_acceleration_var.set(str(control.get("max_acceleration_rad", self.scurve_max_acceleration_var.get())))
        self.scurve_max_jerk_var.set(str(control.get("max_jerk_rad", self.scurve_max_jerk_var.get())))

    def _apply_follow_servo_config(self):
        servo_config = self._get_servo_config(self.follow_servo_id_var.get().strip())
        control = servo_config.get("control", {}) if servo_config else {}
        if not control:
            return
        self.position_planner_max_velocity_var.set(
            str(control.get("max_velocity_rad", self.position_planner_max_velocity_var.get()))
        )
        self.position_planner_max_acceleration_var.set(
            str(control.get("max_acceleration_rad", self.position_planner_max_acceleration_var.get()))
        )
        self.position_planner_max_jerk_var.set(
            str(control.get("max_jerk_rad", self.position_planner_max_jerk_var.get()))
        )
        self.position_planner_gain_var.set(
            str(control.get("position_gain", self.position_planner_gain_var.get()))
        )

    def _apply_eye_control_config(self, control, use_home=False):
        servo_config = self._get_servo_config(control["id_var"].get().strip())
        min_angle, max_angle = self._get_servo_limits(servo_config)
        home_angle = servo_config.get("home_angle_rad") if servo_config else None
        self._apply_scale_limits(
            control.get("scale"),
            control["angle_var"],
            min_angle,
            max_angle,
            vertical=(control["title"] == "Eyes Vertical"),
            home_angle=home_angle,
            use_home=use_home,
        )

    def _should_use_home_for_eye_control(self, control):
        return not bool(control.get("blink"))

    def _on_single_servo_id_changed(self, _event=None):
        self._apply_single_servo_config(use_home=True)
        self._sync_planner_inputs_from_single()
        self._apply_constraint_servo_config()
        self._apply_follow_servo_config()
        self._schedule_mode_preview_update(delay_ms=0)

    def _on_constraint_servo_id_changed(self, _event=None):
        self._apply_constraint_servo_config()

    def _on_follow_servo_id_changed(self, _event=None):
        self._apply_follow_servo_config()

    def _on_eye_control_id_changed(self, control, _event=None):
        self._apply_eye_control_config(control, use_home=self._should_use_home_for_eye_control(control))

    def connect_clicked(self):
        port = self.port_var.get().strip()
        baud_text = self.baud_var.get().strip()
        if not port:
            messagebox.showerror("Missing port", "Please enter a serial port such as COM8.")
            return
        try:
            baud = int(baud_text)
        except ValueError:
            messagebox.showerror("Invalid baud", f"Baud must be an integer, got: {baud_text}")
            return

        self.status_var.set("Connecting...")
        self._run_rpc(
            f"Connect {port} @ {baud}",
            lambda: self.rpc.connect(port, baud),
            on_success=lambda _: self.status_var.set(f"Connected to {port} @ {baud}"),
        )

    def disconnect_clicked(self):
        self.rpc.close()
        self.status_var.set("Disconnected")
        self.last_result_var.set("Result: disconnected")
        self._append_log("Disconnected from serial transport.")

    def send_mode_clicked(self):
        mode_name = self.rpc_mode_var.get()
        mode_value = self._get_motion_mode_value(mode_name)
        self._run_rpc(
            f"setMode({self._get_motion_mode_label(mode_name)})",
            lambda: self.rpc.set_mode(mode_value),
        )

    def send_motion_settings_clicked(self):
        try:
            period_ms = self._parse_int(self.update_period_ms_var, "Period ms")
            if period_ms < 0:
                raise ValueError("Period ms must be >= 0.")
        except ValueError as exc:
            messagebox.showerror("Motion settings", str(exc))
            return

        mode_name = self.rpc_mode_var.get()
        mode_value = self._get_motion_mode_value(mode_name)

        def action():
            mode_result = self.rpc.set_mode(mode_value)
            period_result = self.rpc.set_update_period_ms(period_ms)
            return {"mode": mode_result, "period": period_result}

        self._run_rpc(
            f"setMode({self._get_motion_mode_label(mode_name)}) + setUpdatePeriodMs({period_ms})",
            action,
        )

    def send_single_clicked(self):
        servo_id = self.single_id_var.get().strip()
        if not servo_id:
            messagebox.showerror("Missing ID", "Servo ID is required.")
            return
        angle_rad = self.single_angle_var.get()
        label, action = self._build_single_command(servo_id, angle_rad)
        self._run_rpc(label, action)

    def browse_file_clicked(self):
        file_path = filedialog.askopenfilename(
            title="Select servo YAML config",
            initialdir=str(SERVO_CONFIG_YAML_PATH.parent),
            filetypes=(("YAML files", "*.yaml *.yml"), ("All files", "*.*")),
        )
        if not file_path:
            return

        self.file_local_path_var.set(file_path)
        if not self.file_remote_path_var.get().strip():
            self.file_remote_path_var.set(DEFAULT_REMOTE_SERVO_CONFIG_PATH)
        try:
            size = Path(file_path).stat().st_size
        except OSError:
            self.file_progress_percent_var.set(0.0)
            self.file_progress_var.set(f"Selected: {file_path}")
        else:
            self.file_progress_percent_var.set(0.0)
            self.file_progress_var.set(f"Selected {size} bytes from {Path(file_path).name}")

    def send_file_clicked(self):
        local_path = self.file_local_path_var.get().strip()
        remote_path = self.file_remote_path_var.get().strip()

        if not local_path:
            messagebox.showerror("File upload", "Local file is required.")
            return
        if not remote_path:
            messagebox.showerror("File upload", "Remote path is required.")
            return

        try:
            yaml_path = Path(local_path).expanduser().resolve()
            if yaml_path.suffix.lower() not in {".yaml", ".yml"}:
                raise ValueError("Local config must be a .yaml or .yml file.")
            if not yaml_path.is_file():
                raise FileNotFoundError(f"Local YAML config not found: {yaml_path}")
            chunk_size = self._parse_int(self.file_chunk_size_var, "Chunk bytes")
            if chunk_size <= 0:
                raise ValueError("Chunk bytes must be > 0.")
            yaml_size = yaml_path.stat().st_size
        except (OSError, ValueError) as exc:
            messagebox.showerror("File upload", str(exc))
            return

        if self._file_upload_active:
            messagebox.showerror("File upload", "Another upload is already running.")
            return

        self._file_upload_cancel.clear()
        self._file_upload_active = True
        self.file_status_var.set("Converting YAML...")
        self.file_progress_percent_var.set(0.0)
        self.file_progress_var.set("Waiting for YAML conversion.")

        label = (
            f"convert+fileWrite {yaml_path.name} -> {remote_path} "
            f"(chunk={chunk_size}, yaml_size={yaml_size})"
        )

        def progress(bytes_sent, total_size):
            self.events.put(("file_progress", bytes_sent, total_size))

        def action():
            json_path = self._convert_yaml_config(yaml_path)
            if self._file_upload_cancel.is_set():
                raise InterruptedError("Upload aborted after YAML conversion.")
            self.events.put(("file_conversion_complete", str(json_path), json_path.stat().st_size))
            result = self.rpc.upload_file(
                json_path,
                remote_path,
                chunk_size,
                progress=progress,
                cancelled=self._file_upload_cancel.is_set,
            )
            result["source_yaml"] = str(yaml_path)
            return result

        def on_success(result):
            self._file_upload_active = False
            self.file_status_var.set("Upload complete")
            self.file_progress_percent_var.set(100.0)
            self.file_progress_var.set(
                f"{result['bytes_sent']} / {result['total_size']} bytes in {result['chunk_count']} chunks"
            )

        def on_error(exc):
            self._file_upload_active = False
            if isinstance(exc, InterruptedError):
                self.file_status_var.set("Upload aborted")
                self.file_progress_var.set("Upload aborted before completion.")
                return
            self.file_status_var.set("Upload failed")

        self._run_rpc(label, action, on_success=on_success, on_error=on_error)

    @staticmethod
    def _convert_yaml_config(yaml_path):
        if not YAML_TO_JSON_SCRIPT.is_file():
            raise FileNotFoundError(f"YAML converter not found: {YAML_TO_JSON_SCRIPT}")

        json_path = Path(yaml_path).with_suffix(".json")
        completed = subprocess.run(
            [sys.executable, str(YAML_TO_JSON_SCRIPT), str(yaml_path), str(json_path)],
            cwd=str(YAML_TO_JSON_SCRIPT.parent),
            capture_output=True,
            text=True,
            check=False,
        )
        if completed.returncode != 0:
            detail = completed.stderr.strip() or completed.stdout.strip()
            raise RuntimeError(detail or "YAML conversion failed without an error message.")
        if not json_path.is_file():
            raise FileNotFoundError(f"Converted JSON was not created: {json_path}")
        return json_path

    def abort_file_clicked(self):
        if self._file_upload_active:
            self._file_upload_cancel.set()
            self.file_status_var.set("Aborting upload...")
            self._append_log("Abort requested for current upload.")
            return

        self._run_rpc(
            "fileWriteAbort()",
            lambda: self.rpc.file_write_abort(),
            on_success=lambda _result: self.file_status_var.set("Abort sent"),
        )

    def _send_file_from_event(self, _event):
        self.send_file_clicked()

    def _send_single_from_event(self, _event):
        self.send_single_clicked()

    def _blink_from_event(self, _event):
        self.blink_clicked()

    def _on_single_scale_changed(self, _value):
        self._schedule_mode_preview_update()
        if not self.single_live_drag_var.get():
            return

        servo_id = self.single_id_var.get().strip()
        if not servo_id or not self.rpc.is_connected:
            return

        self._single_live_pending = True
        if self._single_live_job is None:
            self._schedule_single_live_send()

    def _on_single_scale_released(self, _event):
        self._schedule_mode_preview_update(delay_ms=0)
        if self.single_live_drag_var.get():
            if self._single_live_pending and self._single_live_job is None:
                self._schedule_single_live_send()
            return
        self.send_single_clicked()

    def _on_single_live_drag_toggled(self):
        if not self.single_live_drag_var.get():
            self._cancel_single_live_send()
            self._single_live_pending = False
            return
        if self._single_live_pending and self._single_live_job is None:
            self._schedule_single_live_send()

    def _on_motion_mode_changed(self):
        self._select_mode_tab(self.rpc_mode_var.get())
        self._schedule_mode_preview_update(delay_ms=0)

    def _select_mode_tab(self, mode_name):
        if not self.notebook:
            return
        if mode_name == "scurve":
            self.notebook.select(self.tab_frames["scurve"])
        elif mode_name == "follow":
            self.notebook.select(self.tab_frames["follow"])

    def _schedule_mode_preview_update(self, delay_ms=50):
        if self._preview_update_job is not None:
            self.root.after_cancel(self._preview_update_job)
        self._preview_update_job = self.root.after(delay_ms, self._update_mode_preview)

    def _update_mode_preview(self):
        self._preview_update_job = None
        self._sync_planner_inputs_from_single()

        mode_name = self.rpc_mode_var.get()
        if mode_name == "scurve":
            self._plan_scurve(show_errors=False)
        elif mode_name == "follow":
            self._plan_position_planner(show_errors=False)

    def _sync_planner_inputs_from_single(self):
        servo_id = self.single_id_var.get().strip()
        if servo_id:
            self.constraint_servo_id_var.set(servo_id)
            self.follow_servo_id_var.set(servo_id)
            self._apply_constraint_servo_config()
            self._apply_follow_servo_config()

        angle_rad = self.single_angle_var.get()
        angle_text = f"{angle_rad:.4f}"
        self.scurve_end_position_var.set(angle_text)
        self.position_planner_target_position_var.set(angle_text)

    def fill_all_clicked(self):
        value = self.fill_angle_var.get()
        for control in self.eye_controls:
            control["angle_var"].set(value)
        self._append_log(
            f"Filled {len(EYE_CONTROL_SPECS)} visible eye servos with {value:.4f} rad."
        )

    def zero_all_clicked(self):
        for control in self.eye_controls:
            control["angle_var"].set(0.0)
        self._append_log("Visible eye servos set to 0.0000 rad locally.")

    def send_all_clicked(self):
        if not self.rpc.is_connected:
            messagebox.showerror("Eye Servo Layout", "Not connected. Configure serial and click Connect first.")
            return
        try:
            items = self._build_eye_move_items()
        except ValueError as exc:
            messagebox.showerror("Eye Servo Layout", str(exc))
            return
        label = "move([" + ", ".join(f"{servo_id}: {angle_rad:.4f}" for servo_id, angle_rad in items) + "])"
        self.status_var.set("Sending eye move...")
        self._run_rpc(
            label,
            lambda: self.rpc.move(items),
            on_success=lambda _result: self.status_var.set("Eye move sent"),
        )

    def blink_clicked(self):
        if self._blink_active:
            messagebox.showinfo("Blink", "Blink is already in progress.")
            return
        if not self.rpc.is_connected:
            messagebox.showerror("Blink", "Not connected. Configure serial and click Connect first.")
            return

        self._blink_active = True
        self.status_var.set("Blinking...")
        self._start_blink_cycle()

    def stop_blink_clicked(self):
        if not self._blink_active:
            return

        had_pending_cycle = self._blink_loop_job is not None
        if self._blink_loop_job is not None:
            self.root.after_cancel(self._blink_loop_job)
            self._blink_loop_job = None

        self._blink_active = False
        self.status_var.set("Blink stopped" if had_pending_cycle else "Stopping blink...")

    def send_scurve_constraints_clicked(self):
        try:
            servo_id = self.constraint_servo_id_var.get().strip()
            max_velocity = self._parse_float(self.scurve_max_velocity_var, "Max velocity")
            max_acceleration = self._parse_float(self.scurve_max_acceleration_var, "Max accel")
            max_jerk = self._parse_float(self.scurve_max_jerk_var, "Max jerk")
        except ValueError as exc:
            messagebox.showerror("SCurve constraints", str(exc))
            return
        if not servo_id:
            messagebox.showerror("SCurve constraints", "Servo ID is required.")
            return
        self._run_rpc(
            f"setConstraints({servo_id}, {max_velocity:.4f}, {max_acceleration:.4f}, {max_jerk:.4f})",
            lambda: self.rpc.set_constraints(servo_id, max_velocity, max_acceleration, max_jerk),
            on_success=lambda _result: self.scurve_status_var.set("SCurve constraints sent"),
        )

    def plan_scurve_clicked(self):
        self._plan_scurve(show_errors=True)

    def _plan_scurve(self, show_errors):
        try:
            max_velocity = self._parse_float(self.scurve_max_velocity_var, "Max velocity")
            max_acceleration = self._parse_float(self.scurve_max_acceleration_var, "Max accel")
            max_jerk = self._parse_float(self.scurve_max_jerk_var, "Max jerk")
            start_position = self._parse_float(self.scurve_start_position_var, "Start pos")
            end_position = self._parse_float(self.scurve_end_position_var, "End pos")
            start_velocity = self._parse_float(self.scurve_start_velocity_var, "Start vel")
            end_velocity = self._parse_float(self.scurve_end_velocity_var, "End vel")
            sample_dt = self._parse_float(self.scurve_sample_dt_var, "Sample dt")
            if sample_dt <= 0:
                raise ValueError("Sample dt must be > 0.")
            planner = self._ensure_scurve()
            planner.setConstraints(max_velocity, max_acceleration, max_jerk)
            profile = planner.calculateProfile(
                start_position,
                end_position,
                start_velocity,
                end_velocity,
            )
            estimated_points = max(1, int(profile.total_time / sample_dt) + 1)
            if estimated_points > 20000:
                raise ValueError(
                    f"Sample dt is too small for this move. It would create about {estimated_points} points."
                )
            samples = planner.sampleTrajectory(profile, sample_dt)
        except (ValueError, FileNotFoundError, OSError, SCurveError) as exc:
            self.scurve_status_var.set("SCurve error")
            self.scurve_summary_var.set(str(exc))
            self.scurve_segments_var.set("Segments: -")
            self._clear_curve_plots()
            if show_errors:
                messagebox.showerror("SCurve planner", str(exc))
            return

        self.scurve_status_var.set("SCurve ready")
        self.scurve_summary_var.set(
            f"distance={profile.distance:.4f} | total={profile.total_time:.4f}s | "
            f"v_cruise={profile.v_cruise:.4f} | a_limit={profile.a_limit:.4f} | "
            f"samples={len(samples['times'])}"
        )
        self.scurve_segments_var.set(
            "Segments: "
            f"t1={profile.t1:.4f}, t2={profile.t2:.4f}, t3={profile.t3:.4f}, "
            f"t4={profile.t4:.4f}, t5={profile.t5:.4f}, t6={profile.t6:.4f}, t7={profile.t7:.4f}"
        )
        self.scurve_plot_series["position"] = {
            "title": "Position",
            "color": "#7ce0a0",
            "times": samples["times"],
            "values": samples["positions"],
        }
        self.scurve_plot_series["velocity"] = {
            "title": "Velocity",
            "color": "#65c7ff",
            "times": samples["times"],
            "values": samples["velocities"],
        }
        self.scurve_plot_series["acceleration"] = {
            "title": "Acceleration",
            "color": "#ffd166",
            "times": samples["times"],
            "values": samples["accelerations"],
        }
        self.scurve_plot_series["jerk"] = {
            "title": "Jerk",
            "color": "#ff8c7e",
            "times": samples["times"],
            "values": samples["jerks"],
        }
        self._redraw_all_curve_plots()
        self._append_log(
            f"SCurve planned: total={profile.total_time:.4f}s, samples={len(samples['times'])}, "
            f"constraints=({max_velocity:.3f}, {max_acceleration:.3f}, {max_jerk:.3f})"
        )

    def _plan_scurve_from_event(self, _event):
        self.plan_scurve_clicked()

    def send_follow_constraints_clicked(self):
        try:
            servo_id = self.follow_servo_id_var.get().strip()
            max_velocity = self._parse_float(self.position_planner_max_velocity_var, "Max velocity")
            max_acceleration = self._parse_float(self.position_planner_max_acceleration_var, "Max accel")
            max_jerk = self._parse_float(self.position_planner_max_jerk_var, "Max jerk")
        except ValueError as exc:
            messagebox.showerror("Follow planner constraints", str(exc))
            return
        if not servo_id:
            messagebox.showerror("Follow planner constraints", "Servo ID is required.")
            return

        self._run_rpc(
            f"setConstraints({servo_id}, {max_velocity:.4f}, {max_acceleration:.4f}, {max_jerk:.4f})",
            lambda: self.rpc.set_constraints(servo_id, max_velocity, max_acceleration, max_jerk),
            on_success=lambda _result: self.position_planner_status_var.set("Follow constraints sent"),
        )

    def send_follow_position_gain_clicked(self):
        try:
            servo_id = self.follow_servo_id_var.get().strip()
            position_gain = self._parse_float(self.position_planner_gain_var, "Pos gain")
        except ValueError as exc:
            messagebox.showerror("Follow planner gain", str(exc))
            return
        if not servo_id:
            messagebox.showerror("Follow planner gain", "Servo ID is required.")
            return

        self._run_rpc(
            f"setPositionGain({servo_id}, {position_gain:.4f})",
            lambda: self.rpc.set_position_gain(servo_id, position_gain),
            on_success=lambda _result: self.position_planner_status_var.set("Follow gain sent"),
        )

    def plan_position_planner_clicked(self):
        self._plan_position_planner(show_errors=True)

    def _plan_position_planner(self, show_errors):
        try:
            max_velocity = self._parse_float(self.position_planner_max_velocity_var, "Max velocity")
            max_acceleration = self._parse_float(self.position_planner_max_acceleration_var, "Max accel")
            max_jerk = self._parse_float(self.position_planner_max_jerk_var, "Max jerk")
            position_gain = self._parse_float(self.position_planner_gain_var, "Pos gain")
            initial_position = self._parse_float(self.position_planner_initial_position_var, "Initial pos")
            initial_velocity = self._parse_float(self.position_planner_initial_velocity_var, "Initial vel")
            initial_acceleration = self._parse_float(self.position_planner_initial_acceleration_var, "Initial accel")
            target_position = self._parse_float(self.position_planner_target_position_var, "Target pos")
            sample_dt = self._parse_float(self.position_planner_sample_dt_var, "Sample dt")
            duration = self._parse_float(self.position_planner_duration_var, "Duration")
            if sample_dt <= 0:
                raise ValueError("Sample dt must be > 0.")
            if duration < 0:
                raise ValueError("Duration must be >= 0.")
            planner = self._ensure_position_planner()
            planner.setConstraints(max_velocity, max_acceleration, max_jerk)
            planner.setPositionGain(position_gain)
            samples = planner.sampleTrajectory(
                target_position=target_position,
                dt=sample_dt,
                duration=duration,
                initial_position=initial_position,
                initial_velocity=initial_velocity,
                initial_acceleration=initial_acceleration,
                position_gain=position_gain,
            )
        except (ValueError, FileNotFoundError, OSError, SCurveError) as exc:
            self.position_planner_status_var.set("Follow planner error")
            self.position_planner_summary_var.set(str(exc))
            self.position_planner_target_var.set("Target: -")
            self._clear_position_planner_plots()
            if show_errors:
                messagebox.showerror("Follow planner", str(exc))
            return

        final_position = samples["positions"][-1]
        final_velocity = samples["velocities"][-1]
        final_acceleration = samples["accelerations"][-1]
        self.position_planner_status_var.set("Follow planner ready")
        self.position_planner_summary_var.set(
            f"duration={duration:.4f}s | samples={len(samples['times'])} | "
            f"final_pos={final_position:.4f} | final_vel={final_velocity:.4f} | final_acc={final_acceleration:.4f}"
        )
        self.position_planner_target_var.set(
            f"Target: {target_position:.4f} | initial={initial_position:.4f} | gain={position_gain:.4f}"
        )
        self.position_planner_plot_series["position"] = {
            "title": "Position",
            "color": "#6abf69",
            "times": samples["times"],
            "values": samples["positions"],
        }
        self.position_planner_plot_series["velocity"] = {
            "title": "Velocity",
            "color": "#4da3ff",
            "times": samples["times"],
            "values": samples["velocities"],
        }
        self.position_planner_plot_series["acceleration"] = {
            "title": "Acceleration",
            "color": "#f2b43c",
            "times": samples["times"],
            "values": samples["accelerations"],
        }
        self.position_planner_plot_series["jerk"] = {
            "title": "Jerk",
            "color": "#ef6c73",
            "times": samples["times"],
            "values": samples["jerks"],
        }
        self._redraw_all_position_planner_plots()
        self._append_log(
            f"Follow planner: target={target_position:.4f}, duration={duration:.4f}s, "
            f"samples={len(samples['times'])}, constraints=({max_velocity:.3f}, {max_acceleration:.3f}, {max_jerk:.3f})"
        )

    def _plan_position_planner_from_event(self, _event):
        self.plan_position_planner_clicked()

    def _ensure_scurve(self):
        if self.scurve is None:
            self.scurve = SCurve()
        return self.scurve

    def _ensure_position_planner(self):
        if self.position_planner is None:
            self.position_planner = SCurvePositionPlanner1D()
        return self.position_planner

    def _parse_float(self, variable, label):
        try:
            return float(variable.get().strip())
        except ValueError as exc:
            raise ValueError(f"{label} must be a valid number.") from exc

    def _parse_int(self, variable, label):
        try:
            return int(variable.get().strip())
        except ValueError as exc:
            raise ValueError(f"{label} must be a valid integer.") from exc

    def _build_eye_move_items(self):
        items = []
        used_ids = set()

        for control in self.eye_controls:
            servo_id = control["id_var"].get().strip()
            if not servo_id:
                raise ValueError(f"{control['title']} ID is required.")
            if servo_id in used_ids:
                raise ValueError(f"Duplicate eye control ID: {servo_id}.")
            used_ids.add(servo_id)
            items.append((servo_id, float(control["angle_var"].get())))

        return items

    def _build_blink_move_items(self):
        close_items = []
        open_items = []
        used_ids = set()
        has_motion = False

        for control in self.eye_controls:
            if not control.get("blink"):
                continue

            servo_id = control["id_var"].get().strip()
            if not servo_id:
                raise ValueError(f"{control['title']} ID is required for blink.")
            if servo_id in used_ids:
                raise ValueError(f"Duplicate blink servo ID: {servo_id}.")

            servo_config = self._get_servo_config(servo_id)
            if not servo_config:
                raise ValueError(f"Blink servo config not found for ID: {servo_id}.")
            if "home_angle_rad" not in servo_config:
                raise ValueError(f"Blink servo {servo_id} is missing home_angle_rad.")

            used_ids.add(servo_id)
            min_angle, max_angle = self._get_servo_limits(servo_config)
            close_angle = max(min_angle, min(max_angle, float(servo_config["home_angle_rad"])))
            open_angle = max(min_angle, min(max_angle, float(control["angle_var"].get())))
            if abs(open_angle - close_angle) > 1e-6:
                has_motion = True
            close_items.append((servo_id, close_angle))
            open_items.append((servo_id, open_angle))

        if not close_items:
            raise ValueError("No blink-enabled eye servos are configured.")
        if not has_motion:
            raise ValueError("Blink open angles match home_angle_rad. Adjust the eyelid sliders to an open-eye position first.")

        return close_items, open_items

    def _build_blink_transition_frames(self, start_items, end_items):
        if len(start_items) != len(end_items):
            raise ValueError("Blink frame build failed: item count mismatch.")

        steps = max(1, int(math.ceil(BLINK_TRANSITION_MS / BLINK_STEP_MS)))
        frames = []
        for step_index in range(1, steps + 1):
            ratio = step_index / steps
            frame = []
            for (start_id, start_angle), (end_id, end_angle) in zip(start_items, end_items):
                if start_id != end_id:
                    raise ValueError("Blink frame build failed: servo order mismatch.")
                angle = start_angle + (end_angle - start_angle) * ratio
                frame.append((start_id, angle))
            frames.append(frame)
        return frames

    def _start_blink_cycle(self):
        if not self._blink_active:
            return

        try:
            hold_ms = self._parse_int(self.blink_hold_ms_var, "Blink delay ms")
            if hold_ms < 0:
                raise ValueError("Blink delay ms must be >= 0.")
            close_items, open_items = self._build_blink_move_items()
        except ValueError as exc:
            self._blink_active = False
            self.status_var.set("Blink failed")
            messagebox.showerror("Blink", str(exc))
            return

        self._blink_loop_job = None
        self._run_rpc(
            f"blink(delay={hold_ms} ms)",
            lambda: self._run_blink_sequence(close_items, open_items, hold_ms),
            on_success=lambda result, delay_ms=hold_ms: self._on_blink_success(result, delay_ms),
            on_error=self._on_blink_error,
        )

    def _schedule_blink_cycle(self, delay_ms):
        if not self._blink_active:
            return
        if self._blink_loop_job is not None:
            self.root.after_cancel(self._blink_loop_job)
        self._blink_loop_job = self.root.after(delay_ms, self._start_blink_cycle)

    def _run_blink_sequence(self, close_items, open_items, hold_ms):
        close_frames = self._build_blink_transition_frames(open_items, close_items)
        open_frames = self._build_blink_transition_frames(close_items, open_items)
        step_sleep_s = BLINK_STEP_MS / 1000.0

        close_result = 0
        for frame in close_frames:
            close_result = self.rpc.move(frame)
            if step_sleep_s > 0:
                time.sleep(step_sleep_s)
        if hold_ms > 0:
            time.sleep(hold_ms / 1000.0)
        open_result = 0
        for frame in open_frames:
            open_result = self.rpc.move(frame)
            if step_sleep_s > 0:
                time.sleep(step_sleep_s)
        return {
            "close_result": close_result,
            "open_result": open_result,
            "hold_ms": hold_ms,
        }

    def _on_blink_success(self, _result, delay_ms):
        if not self._blink_active:
            self.status_var.set("Blink stopped")
            return
        self.status_var.set("Blinking...")
        self._schedule_blink_cycle(delay_ms)

    def _on_blink_error(self, exc):
        self._blink_active = False
        if self._blink_loop_job is not None:
            self.root.after_cancel(self._blink_loop_job)
            self._blink_loop_job = None
        self.status_var.set("Blink failed")
        messagebox.showerror("Blink", str(exc))

    def _get_single_live_interval_ms(self):
        try:
            rate_hz = float(self.single_live_rate_hz_var.get().strip())
        except ValueError:
            rate_hz = 50.0
        if rate_hz <= 0:
            rate_hz = 50.0
        return max(1, int(round(1000.0 / rate_hz)))

    def _schedule_single_live_send(self):
        interval_ms = self._get_single_live_interval_ms()
        self._single_live_job = self.root.after(interval_ms, self._flush_single_live_send)

    def _flush_single_live_send(self):
        self._single_live_job = None
        if not self.single_live_drag_var.get() or not self._single_live_pending:
            return

        servo_id = self.single_id_var.get().strip()
        if not servo_id or not self.rpc.is_connected:
            self._single_live_pending = False
            return

        angle_rad = self.single_angle_var.get()
        label, action = self._build_single_command(servo_id, angle_rad)
        self._single_live_pending = False
        self._run_rpc(label, action, log_request=False)

        if self.single_live_drag_var.get() and self._single_live_pending:
            self._schedule_single_live_send()

    def _cancel_single_live_send(self):
        if self._single_live_job is not None:
            self.root.after_cancel(self._single_live_job)
            self._single_live_job = None

    def _get_motion_mode_value(self, mode_name):
        if mode_name == "follow":
            return servo_service_common.RpcMotionMode.RpcMotionModeFollow
        if mode_name == "scurve":
            return servo_service_common.RpcMotionMode.RpcMotionModeSCurve
        return servo_service_common.RpcMotionMode.RpcMotionModeImmediate

    def _get_motion_mode_label(self, mode_name):
        if mode_name == "follow":
            return "Follow"
        if mode_name == "scurve":
            return "SCurve"
        return "Immediate"

    def _build_single_command(self, servo_id, angle_rad):
        return (
            f"move([{servo_id}: {angle_rad:.4f} rad])",
            lambda: self.rpc.move([(servo_id, angle_rad)]),
        )

    def _clear_curve_plots(self):
        for key, series in self.scurve_plot_series.items():
            series["times"] = []
            series["values"] = []
            self._redraw_curve_plot(key)

    def _clear_position_planner_plots(self):
        for key, series in self.position_planner_plot_series.items():
            series["times"] = []
            series["values"] = []
            self._redraw_position_planner_plot(key)

    def _redraw_all_curve_plots(self):
        for key in self.scurve_canvases:
            self._redraw_curve_plot(key)

    def _redraw_all_position_planner_plots(self):
        for key in self.position_planner_canvases:
            self._redraw_position_planner_plot(key)

    def _redraw_curve_plot(self, key):
        canvas = self.scurve_canvases.get(key)
        series = self.scurve_plot_series.get(key)
        self._render_plot_series(canvas, series)

    def _redraw_position_planner_plot(self, key):
        canvas = self.position_planner_canvases.get(key)
        series = self.position_planner_plot_series.get(key)
        self._render_plot_series(canvas, series)

    def _render_plot_series(self, canvas, series):
        if not canvas or not series:
            return

        canvas.delete("all")
        width = max(canvas.winfo_width(), 10)
        height = max(canvas.winfo_height(), 10)
        if width < 80 or height < 80:
            return

        if not series["times"] or not series["values"]:
            canvas.create_text(
                width / 2,
                height / 2,
                text="Plan curve to preview.",
                fill=self.colors["muted_text"],
                font=("Segoe UI", 11),
            )
            return

        left = 60
        top = 16
        right = width - 18
        bottom = height - 30
        plot_width = max(1, right - left)
        plot_height = max(1, bottom - top)
        times = series["times"]
        values = series["values"]
        min_time = times[0]
        max_time = times[-1] if times[-1] > times[0] else times[0] + 1.0
        min_value = min(values)
        max_value = max(values)
        if abs(max_value - min_value) < 1e-9:
            padding = max(1.0, abs(max_value) * 0.1 + 1.0)
            min_value -= padding
            max_value += padding
        else:
            padding = (max_value - min_value) * 0.1
            min_value -= padding
            max_value += padding

        for step in range(6):
            x = left + plot_width * step / 5
            y = top + plot_height * step / 5
            canvas.create_line(x, top, x, bottom, fill=self.colors["plot_grid"], width=1)
            canvas.create_line(left, y, right, y, fill=self.colors["plot_grid"], width=1)

        if min_value < 0 < max_value:
            zero_y = top + (max_value / (max_value - min_value)) * plot_height
            canvas.create_line(left, zero_y, right, zero_y, fill=self.colors["plot_axis"], width=1)

        canvas.create_rectangle(left, top, right, bottom, outline=self.colors["plot_axis"], width=1)

        points = []
        value_span = max_value - min_value
        time_span = max_time - min_time
        for t_value, y_value in zip(times, values):
            x = left + ((t_value - min_time) / time_span) * plot_width
            y = bottom - ((y_value - min_value) / value_span) * plot_height
            points.extend((x, y))

        if len(points) >= 4:
            canvas.create_line(*points, fill=series["color"], width=2)

        canvas.create_text(left, top - 8, anchor="sw", text=series["title"], fill=self.colors["hero_text"], font=("Segoe UI Semibold", 10))
        canvas.create_text(left - 8, top, anchor="ne", text=f"{max_value:.4f}", fill=self.colors["muted_text"], font=("Consolas", 9))
        canvas.create_text(left - 8, bottom, anchor="ne", text=f"{min_value:.4f}", fill=self.colors["muted_text"], font=("Consolas", 9))
        canvas.create_text(left, bottom + 8, anchor="nw", text=f"{min_time:.4f}s", fill=self.colors["muted_text"], font=("Consolas", 9))
        canvas.create_text(right, bottom + 8, anchor="ne", text=f"{max_time:.4f}s", fill=self.colors["muted_text"], font=("Consolas", 9))

    def _run_rpc(self, label, action, on_success=None, on_error=None, log_request=True):
        if log_request:
            self._append_log(f"> {label}")

        def worker():
            started_at = time.perf_counter()
            try:
                result = action()
            except Exception as exc:
                elapsed_ms = (time.perf_counter() - started_at) * 1000.0
                self.events.put(("error", label, exc, traceback.format_exc(), elapsed_ms, on_error))
            else:
                elapsed_ms = (time.perf_counter() - started_at) * 1000.0
                self.events.put(("success", label, result, on_success, elapsed_ms))

        threading.Thread(target=worker, daemon=True).start()

    def _poll_events(self):
        while True:
            try:
                event = self.events.get_nowait()
            except queue.Empty:
                break
            self._handle_event(event)
        self.root.after(120, self._poll_events)

    def _handle_event(self, event):
        kind = event[0]
        if kind == "file_conversion_complete":
            _, json_path, total_size = event
            self.file_status_var.set("Uploading JSON...")
            self.file_progress_var.set(f"Converted {Path(json_path).name}: {total_size} bytes")
            self._append_log(f"Converted YAML to {json_path} ({total_size} bytes)")
            return

        if kind == "file_progress":
            _, bytes_sent, total_size = event
            percent = (bytes_sent / total_size * 100.0) if total_size else 100.0
            self.file_progress_percent_var.set(percent)
            self.file_status_var.set(
                f"Uploading... {bytes_sent}/{total_size} bytes"
            )
            self.file_progress_var.set(
                f"{bytes_sent} / {total_size} bytes ({percent:.1f}%)"
            )
            return

        if kind == "success":
            _, label, result, on_success, elapsed_ms = event
            self.last_result_var.set(f"Result: {result} ({elapsed_ms:.3f} ms)")
            self._append_log(f"< {label} -> {result} [{elapsed_ms:.3f} ms]")
            if callable(on_success):
                on_success(result)
            return

        _, label, exc, trace_text, elapsed_ms, on_error = event
        self.status_var.set("Error")
        self.last_result_var.set(f"Result: error ({exc}) [{elapsed_ms:.3f} ms]")
        self._append_log(f"! {label} failed: {exc} [{elapsed_ms:.3f} ms]")
        self._append_log(trace_text.strip())
        if callable(on_error):
            on_error(exc)

    def _append_log(self, text):
        self.log_widget.configure(state="normal")
        self.log_widget.insert("end", text + "\n")
        self.log_widget.see("end")
        self.log_widget.configure(state="disabled")

    def on_close(self):
        self.rpc.close()
        self.root.destroy()


def main():
    root = tk.Tk()
    app = FaceServoControlApp(root)
    root.mainloop()


if __name__ == "__main__":
    main()
