host.controller_config.segment_editor

  1"""Segment editor dialog for analog piecewise-linear response curves.
  2
  3Visual piecewise-linear editor that maps joystick input (X: -1 to 1)
  4to output (Y: -1 to 1). Users can add, remove, and drag control points
  5to shape the response curve. Values between points are linearly
  6interpolated, allowing different slopes in different regions.
  7
  8Control point data format (stored in ActionDefinition.extra)::
  9
 10    action.extra["segment_points"] = [
 11        {"x": -1.0, "y": -1.0},
 12        {"x":  0.0, "y":  0.0},
 13        {"x":  1.0, "y":  1.0},
 14    ]
 15
 16Endpoints (x=-1 and x=1) are always present; intermediate points
 17can be added or removed freely.
 18"""
 19
 20import math
 21import tkinter as tk
 22from copy import deepcopy
 23from tkinter import ttk, filedialog, messagebox
 24
 25import yaml
 26
 27from host.controller_config.colors import (
 28    BG_WHITE,
 29    CURVE_LINE,
 30    ENDPOINT_FILL,
 31    POINT_FILL,
 32    POINT_OUTLINE,
 33)
 34from host.controller_config.editor_utils import (
 35    DIALOG_H,
 36    DIALOG_MARGIN,
 37    DIALOG_PLOT_H,
 38    DIALOG_PLOT_W,
 39    DIALOG_W,
 40    UndoStack,
 41    draw_editor_grid,
 42)
 43
 44# Visual sizes (pixels)
 45_POINT_RADIUS = 7
 46
 47# Minimum gap between adjacent control point X positions
 48_MIN_X_GAP = 0.04
 49
 50_DEFAULT_STATUS = ("Click to add point | Right-click to remove | "
 51                   "Drag to adjust")
 52
 53
 54# ---------------------------------------------------------------------------
 55#   Piecewise-linear evaluation (canonical in utils/math/curves.py)
 56# ---------------------------------------------------------------------------
 57
 58from utils.math.curves import default_segment_points
 59
 60
 61# ---------------------------------------------------------------------------
 62#   SegmentEditorDialog
 63# ---------------------------------------------------------------------------
 64
 65class SegmentEditorDialog(tk.Toplevel):
 66    """Modal dialog for visually editing a piecewise-linear response curve.
 67
 68    Interactions:
 69      - Left-click on empty space: add a new control point
 70      - Right-click on a point: remove it (not endpoints)
 71      - Drag a control point: move Y (endpoints) or X+Y (intermediate)
 72
 73    Options:
 74      - Symmetry: odd symmetry — edit positive side, negative mirrors
 75      - Monotonic: Y values must increase left-to-right (enabled by default)
 76    """
 77
 78    def __init__(self, parent, points: list[dict],
 79                 other_curves: dict[str, list[dict]] | None = None,
 80                 scale: float = 1.0, inversion: bool = False,
 81                 allow_nonmono: bool = True):
 82        """
 83        Args:
 84            parent: parent window
 85            points: initial control points
 86            other_curves: optional {action_name: points} for "Copy from..."
 87            scale: action scale factor for processed display
 88            inversion: action inversion flag for processed display
 89        """
 90        super().__init__(parent)
 91        self.title("Segmented Response Curve Editor")
 92        self.transient(parent)
 93        self.resizable(False, False)
 94
 95        self._points = [dict(p) for p in points]
 96        self._points.sort(key=lambda p: p["x"])
 97        self._result = None
 98        self._symmetric = False
 99        self._monotonic = True
100        self._other_curves = other_curves or {}
101        self._scale = scale
102        self._inversion = inversion
103        self._show_processed = False
104        self._allow_nonmono = allow_nonmono
105
106        # Drag state
107        self._drag_idx = None
108
109        # Undo stack (max 30 snapshots)
110        self._undo = UndoStack()
111        self._drag_undo_pushed = False
112
113        self._build_ui()
114        self._draw()
115
116        # Center on the parent window
117        self.update_idletasks()
118        pw = parent.winfo_width()
119        ph = parent.winfo_height()
120        px = parent.winfo_rootx()
121        py = parent.winfo_rooty()
122        dw = self.winfo_reqwidth()
123        dh = self.winfo_reqheight()
124        x = px + (pw - dw) // 2
125        y = py + (ph - dh) // 2
126        self.geometry(f"+{x}+{y}")
127
128        self.grab_set()
129        self.protocol("WM_DELETE_WINDOW", self._on_cancel)
130        self.focus_set()
131
132    def get_result(self) -> list[dict] | None:
133        """Block until dialog closes. Returns points list or None."""
134        self.wait_window()
135        return self._result
136
137    # ------------------------------------------------------------------
138    # Display scale
139    # ------------------------------------------------------------------
140
141    @property
142    def _display_scale(self) -> float:
143        """Scale factor for displayed Y values when processed view is on."""
144        if self._show_processed:
145            s = self._scale
146            if self._inversion:
147                s = -s
148            return s
149        return 1.0
150
151    # ------------------------------------------------------------------
152    # Undo
153    # ------------------------------------------------------------------
154
155    def _push_undo(self):
156        """Save current points to undo stack."""
157        self._undo.push(self._points)
158
159    def _pop_undo(self):
160        """Restore previous points from undo stack."""
161        state = self._undo.pop()
162        if state is None:
163            self._status_var.set("Nothing to undo")
164            return
165        self._points = state
166        self._draw()
167        self._status_var.set(f"Undo ({len(self._undo)} remaining)")
168
169    # ------------------------------------------------------------------
170    # Import / Export / Copy
171    # ------------------------------------------------------------------
172
173    def _on_export(self):
174        """Export current curve points to a YAML file."""
175        path = filedialog.asksaveasfilename(
176            parent=self, title="Export Segment Curve",
177            defaultextension=".yaml",
178            filetypes=[("YAML files", "*.yaml *.yml"), ("All files", "*.*")])
179        if not path:
180            return
181        data = {"type": "segment", "points": deepcopy(self._points)}
182        with open(path, "w") as f:
183            yaml.dump(data, f, default_flow_style=False, sort_keys=False)
184        self._status_var.set(f"Exported to {path}")
185
186    def _on_import(self):
187        """Import curve points from a YAML file."""
188        path = filedialog.askopenfilename(
189            parent=self, title="Import Segment Curve",
190            filetypes=[("YAML files", "*.yaml *.yml"), ("All files", "*.*")])
191        if not path:
192            return
193        try:
194            with open(path) as f:
195                data = yaml.safe_load(f)
196        except Exception as exc:
197            messagebox.showerror("Import Failed",
198                                 f"Could not read YAML file:\n{exc}",
199                                 parent=self)
200            return
201
202        if isinstance(data, dict):
203            points = data.get("points", [])
204        elif isinstance(data, list):
205            points = data
206        else:
207            messagebox.showerror(
208                "Import Failed",
209                "File does not contain curve data.\n"
210                "Expected a 'points' list of {{x, y}} entries.",
211                parent=self)
212            return
213
214        if not points:
215            messagebox.showerror(
216                "Import Failed",
217                "No points found in file.\n"
218                "Expected a 'points' key containing a list.",
219                parent=self)
220            return
221
222        if not isinstance(points, list) or not all(
223                isinstance(p, dict) and "x" in p and "y" in p
224                for p in points):
225            messagebox.showerror(
226                "Import Failed",
227                "Invalid point data. Each point must have "
228                "'x' and 'y' fields.",
229                parent=self)
230            return
231
232        self._push_undo()
233        # Strip tangent field — segment points don't use it
234        self._points = [{"x": p["x"], "y": p["y"]} for p in points]
235        self._points.sort(key=lambda p: p["x"])
236        self._draw()
237        self._status_var.set(f"Imported from {path}")
238
239    def _on_copy_from(self):
240        """Copy curve data from another action."""
241        if not self._other_curves:
242            return
243        win = tk.Toplevel(self)
244        win.title("Copy Curve From...")
245        win.transient(self)
246        win.grab_set()
247        win.resizable(False, False)
248
249        ttk.Label(win, text="Select an action to copy its curve:",
250                  padding=5).pack(anchor=tk.W)
251        listbox = tk.Listbox(win, height=min(10, len(self._other_curves)),
252                             width=40)
253        listbox.pack(padx=10, pady=5, fill=tk.BOTH, expand=True)
254        names = sorted(self._other_curves.keys())
255        for name in names:
256            listbox.insert(tk.END, name)
257
258        def on_ok():
259            sel = listbox.curselection()
260            if not sel:
261                return
262            chosen = names[sel[0]]
263            pts = self._other_curves[chosen]
264            self._push_undo()
265            self._points = [{"x": p["x"], "y": p["y"]} for p in pts]
266            self._points.sort(key=lambda p: p["x"])
267            self._draw()
268            self._status_var.set(f"Copied curve from {chosen}")
269            win.destroy()
270
271        listbox.bind("<Double-1>", lambda e: on_ok())
272        bf = ttk.Frame(win)
273        bf.pack(fill=tk.X, padx=10, pady=(0, 10))
274        ttk.Button(bf, text="OK", command=on_ok).pack(side=tk.RIGHT, padx=5)
275        ttk.Button(bf, text="Cancel",
276                   command=win.destroy).pack(side=tk.RIGHT)
277
278        # Center on parent editor dialog
279        win.update_idletasks()
280        px, py = self.winfo_rootx(), self.winfo_rooty()
281        pw, ph = self.winfo_width(), self.winfo_height()
282        ww, wh = win.winfo_width(), win.winfo_height()
283        win.geometry(f"+{px + (pw - ww) // 2}+{py + (ph - wh) // 2}")
284
285    # ------------------------------------------------------------------
286    # UI
287    # ------------------------------------------------------------------
288
289    def _build_ui(self):
290        self._canvas = tk.Canvas(
291            self, width=DIALOG_W, height=DIALOG_H,
292            bg=BG_WHITE, cursor="crosshair")
293        self._canvas.pack(padx=10, pady=(10, 5))
294
295        self._canvas.bind("<ButtonPress-1>", self._on_press)
296        self._canvas.bind("<B1-Motion>", self._on_drag)
297        self._canvas.bind("<ButtonRelease-1>", self._on_release)
298        self._canvas.bind("<Button-3>", self._on_right_click)
299
300        self._status_var = tk.StringVar(value=_DEFAULT_STATUS)
301        ttk.Label(self, textvariable=self._status_var,
302                  relief=tk.SUNKEN, anchor=tk.W).pack(fill=tk.X, padx=10)
303
304        # Top button row: file and copy operations
305        top_btn = ttk.Frame(self)
306        top_btn.pack(fill=tk.X, padx=10, pady=(5, 0))
307        ttk.Button(top_btn, text="Export YAML",
308                   command=self._on_export).pack(side=tk.LEFT, padx=5)
309        ttk.Button(top_btn, text="Import YAML",
310                   command=self._on_import).pack(side=tk.LEFT, padx=5)
311        if self._other_curves:
312            ttk.Button(top_btn, text="Copy from...",
313                       command=self._on_copy_from).pack(side=tk.LEFT, padx=5)
314
315        # Bottom button row: edit operations
316        btn = ttk.Frame(self)
317        btn.pack(fill=tk.X, padx=10, pady=(5, 10))
318        ttk.Button(btn, text="Reset to Linear",
319                   command=self._on_reset).pack(side=tk.LEFT, padx=5)
320        ttk.Button(btn, text="Undo",
321                   command=self._pop_undo).pack(side=tk.LEFT, padx=5)
322        self._sym_var = tk.BooleanVar()
323        ttk.Checkbutton(btn, text="Symmetry", variable=self._sym_var,
324                        command=self._on_symmetry_toggle
325                        ).pack(side=tk.LEFT, padx=5)
326        self._mono_var = tk.BooleanVar(value=True)
327        mono_state = "normal" if self._allow_nonmono else "disabled"
328        ttk.Checkbutton(btn, text="Monotonic", variable=self._mono_var,
329                        command=self._on_monotonic_toggle,
330                        state=mono_state
331                        ).pack(side=tk.LEFT, padx=5)
332        self._proc_var = tk.BooleanVar(value=False)
333        self._proc_cb = ttk.Checkbutton(
334            btn, text="Show Processed",
335            variable=self._proc_var,
336            command=self._on_processed_toggle)
337        self._proc_cb.pack(side=tk.LEFT, padx=5)
338        ttk.Button(btn, text="Cancel",
339                   command=self._on_cancel).pack(side=tk.RIGHT, padx=5)
340        ttk.Button(btn, text="OK",
341                   command=self._on_ok).pack(side=tk.RIGHT, padx=5)
342
343        self.bind("<Control-z>", lambda e: self._pop_undo())
344
345    # ------------------------------------------------------------------
346    # Coordinate conversion
347    # ------------------------------------------------------------------
348
349    @property
350    def _y_extent(self) -> float:
351        """Half-range for Y axis, expanded to fit scaled values."""
352        return max(1.0, abs(self._display_scale))
353
354    def _d2c(self, x: float, y: float) -> tuple[float, float]:
355        """Data to canvas pixels."""
356        ext = self._y_extent
357        cx = DIALOG_MARGIN + (x + 1) / 2 * DIALOG_PLOT_W
358        cy = DIALOG_MARGIN + (ext - y) / (2 * ext) * DIALOG_PLOT_H
359        return cx, cy
360
361    def _c2d(self, cx: float, cy: float) -> tuple[float, float]:
362        """Canvas pixels to data."""
363        ext = self._y_extent
364        x = (cx - DIALOG_MARGIN) / DIALOG_PLOT_W * 2 - 1
365        y = ext - (cy - DIALOG_MARGIN) / DIALOG_PLOT_H * (2 * ext)
366        return x, y
367
368    # ------------------------------------------------------------------
369    # Drawing
370    # ------------------------------------------------------------------
371
372    def _draw(self):
373        c = self._canvas
374        c.delete("all")
375        self._draw_grid()
376        self._draw_curve()
377        self._draw_points()
378
379    def _draw_grid(self):
380        ext = self._y_extent
381        draw_editor_grid(self._canvas, self._d2c,
382                         DIALOG_MARGIN, DIALOG_PLOT_W, DIALOG_PLOT_H,
383                         DIALOG_W, DIALOG_H,
384                         y_min=-ext, y_max=ext)
385
386    def _draw_curve(self):
387        pts = self._points
388        if len(pts) < 2:
389            return
390        s = self._display_scale
391        coords = []
392        for pt in pts:
393            cx, cy = self._d2c(pt["x"], pt["y"] * s)
394            coords.extend([cx, cy])
395        if len(coords) >= 4:
396            self._canvas.create_line(
397                *coords, fill=CURVE_LINE, width=2, smooth=False)
398
399    def _draw_points(self):
400        c = self._canvas
401        s = self._display_scale
402        for i, pt in enumerate(self._points):
403            cx, cy = self._d2c(pt["x"], pt["y"] * s)
404            is_endpoint = (i == 0 or i == len(self._points) - 1)
405            is_mirror = (self._symmetric
406                         and pt["x"] < -_MIN_X_GAP / 2)
407            if is_mirror:
408                fill = "#c0a0a0"
409            elif is_endpoint:
410                fill = ENDPOINT_FILL
411            else:
412                fill = POINT_FILL
413            c.create_oval(cx - _POINT_RADIUS, cy - _POINT_RADIUS,
414                          cx + _POINT_RADIUS, cy + _POINT_RADIUS,
415                          fill=fill, outline=POINT_OUTLINE, width=2)
416
417    # ------------------------------------------------------------------
418    # Hit testing & interaction
419    # ------------------------------------------------------------------
420
421    def _hit_test(self, cx, cy):
422        """Find what point is at canvas position (cx, cy).
423
424        Returns index or None.
425        When symmetry is on, negative-side mirrors are not interactive.
426        """
427        s = self._display_scale
428        for i, pt in enumerate(self._points):
429            if self._symmetric and pt["x"] < -_MIN_X_GAP / 2:
430                continue
431            px, py = self._d2c(pt["x"], pt["y"] * s)
432            if math.hypot(cx - px, cy - py) <= _POINT_RADIUS + 3:
433                return i
434        return None
435
436    def _is_endpoint(self, idx: int) -> bool:
437        return idx == 0 or idx == len(self._points) - 1
438
439    def _add_point_at(self, cx, cy):
440        """Add a new control point at canvas position."""
441        x, vis_y = self._c2d(cx, cy)
442
443        if self._symmetric and x < -_MIN_X_GAP / 2:
444            self._status_var.set(
445                "Add points on the positive side (symmetry)")
446            return
447
448        x_min = self._points[0]["x"]
449        x_max = self._points[-1]["x"]
450        if x <= x_min + _MIN_X_GAP or x >= x_max - _MIN_X_GAP:
451            return
452        # Un-scale the visual Y to get the raw point value
453        s = self._display_scale
454        y = vis_y / s if abs(s) > 1e-6 else vis_y
455        y = max(-1.0, min(1.0, y))
456
457        for pt in self._points:
458            if abs(pt["x"] - x) < _MIN_X_GAP:
459                return
460
461        if self._monotonic:
462            y = self._clamp_monotonic_insert(x, y)
463
464        self._push_undo()
465        self._points.append({
466            "x": round(x, 3),
467            "y": round(y, 3),
468        })
469        self._points.sort(key=lambda p: p["x"])
470
471        if self._symmetric:
472            self._enforce_symmetry()
473
474        self._draw()
475        self._status_var.set(
476            f"Added point at x={x:.2f} ({len(self._points)} points)")
477
478    def _remove_point(self, idx):
479        """Remove the control point at *idx* (not endpoints)."""
480        if self._is_endpoint(idx) or len(self._points) <= 2:
481            return
482        self._push_undo()
483        self._points.pop(idx)
484        if self._symmetric:
485            self._enforce_symmetry()
486        self._draw()
487        self._status_var.set(
488            f"Removed point ({len(self._points)} points)")
489
490    def _on_press(self, event):
491        hit = self._hit_test(event.x, event.y)
492        if hit is not None:
493            self._drag_idx = hit
494            self._drag_undo_pushed = False
495        else:
496            self._drag_idx = None
497            self._add_point_at(event.x, event.y)
498
499    def _on_drag(self, event):
500        if self._drag_idx is None:
501            return
502        if not self._drag_undo_pushed:
503            self._push_undo()
504            self._drag_undo_pushed = True
505        i = self._drag_idx
506        pt = self._points[i]
507
508        _, vis_y = self._c2d(event.x, event.y)
509        s = self._display_scale
510        raw_y = vis_y / s if abs(s) > 1e-6 else vis_y
511        if self._symmetric and abs(pt["x"]) < _MIN_X_GAP / 2:
512            pt["y"] = 0.0
513        else:
514            raw_y = max(-1.0, min(1.0, raw_y))
515            if self._monotonic:
516                raw_y = self._clamp_monotonic(i, raw_y)
517            pt["y"] = round(raw_y, 3)
518
519        if not self._is_endpoint(i):
520            x, _ = self._c2d(event.x, event.y)
521            x_lo = self._points[i - 1]["x"] + _MIN_X_GAP
522            x_hi = self._points[i + 1]["x"] - _MIN_X_GAP
523            if self._symmetric and pt["x"] > 0:
524                x_lo = max(x_lo, _MIN_X_GAP)
525            pt["x"] = round(max(x_lo, min(x_hi, x)), 3)
526
527            # Re-enforce monotonic after X move changes neighbors
528            if self._monotonic:
529                y_clamped = self._clamp_monotonic(i, pt["y"])
530                pt["y"] = round(y_clamped, 3)
531
532        if self._symmetric:
533            self._enforce_symmetry()
534            for j, p in enumerate(self._points):
535                if p is pt:
536                    self._drag_idx = j
537                    break
538
539        self._status_var.set(
540            f"Point {self._drag_idx}: x={pt['x']:.2f}  y={pt['y']:.3f}")
541        self._draw()
542
543    def _on_release(self, event):
544        self._drag_idx = None
545        self._status_var.set(_DEFAULT_STATUS)
546
547    def _on_right_click(self, event):
548        """Remove a control point on right-click."""
549        hit = self._hit_test(event.x, event.y)
550        if hit is None:
551            return
552        if self._is_endpoint(hit):
553            self._status_var.set("Cannot remove endpoints")
554            return
555        if len(self._points) <= 2:
556            self._status_var.set("Need at least 2 points")
557            return
558        self._drag_idx = None
559        self._remove_point(hit)
560
561    # ------------------------------------------------------------------
562    # Monotonic constraint
563    # ------------------------------------------------------------------
564
565    def _clamp_monotonic(self, idx: int, y: float) -> float:
566        """Clamp *y* so the curve stays monotonically increasing."""
567        if idx > 0:
568            y = max(y, self._points[idx - 1]["y"])
569        if idx < len(self._points) - 1:
570            y = min(y, self._points[idx + 1]["y"])
571        return y
572
573    def _clamp_monotonic_insert(self, x: float, y: float) -> float:
574        """Clamp *y* for a new point at *x* to maintain monotonicity."""
575        lo_y = -1.0
576        hi_y = 1.0
577        for pt in self._points:
578            if pt["x"] < x:
579                lo_y = max(lo_y, pt["y"])
580            elif pt["x"] > x:
581                hi_y = min(hi_y, pt["y"])
582                break
583        return max(lo_y, min(hi_y, y))
584
585    def _enforce_monotonic(self):
586        """Fix all points to be monotonically increasing in Y."""
587        for i in range(1, len(self._points)):
588            if self._points[i]["y"] < self._points[i - 1]["y"]:
589                self._points[i]["y"] = self._points[i - 1]["y"]
590
591    # ------------------------------------------------------------------
592    # Symmetry
593    # ------------------------------------------------------------------
594
595    def _on_symmetry_toggle(self):
596        """Handle the Symmetry checkbox toggle."""
597        self._push_undo()
598        self._symmetric = self._sym_var.get()
599        if self._symmetric:
600            self._enforce_symmetry()
601            self._draw()
602            self._status_var.set("Symmetry on — edit positive side")
603
604    def _enforce_symmetry(self):
605        """Rebuild negative-side points as odd mirrors of positive side."""
606        positive = [pt for pt in self._points
607                    if pt["x"] > _MIN_X_GAP / 2]
608        center = None
609        for pt in self._points:
610            if abs(pt["x"]) < _MIN_X_GAP / 2:
611                center = pt
612                break
613
614        if center is None:
615            center = {"x": 0.0, "y": 0.0}
616        else:
617            center["x"] = 0.0
618            center["y"] = 0.0
619
620        new_points = []
621        for pt in reversed(positive):
622            new_points.append({
623                "x": round(-pt["x"], 3),
624                "y": round(-pt["y"], 3),
625            })
626        new_points.append(center)
627        new_points.extend(positive)
628        self._points = new_points
629
630    # ------------------------------------------------------------------
631    # Processed view toggle
632    # ------------------------------------------------------------------
633
634    def _on_processed_toggle(self):
635        """Toggle display of scale and inversion on the curve."""
636        self._show_processed = self._proc_var.get()
637        self._draw()
638        if self._show_processed:
639            parts = []
640            if self._inversion:
641                parts.append("inverted")
642            if self._scale != 1.0:
643                parts.append(f"scale={self._scale}")
644            detail = ", ".join(parts) if parts else "no change"
645            self._status_var.set(f"Processed view ({detail})")
646        else:
647            self._status_var.set("Raw view")
648
649    # ------------------------------------------------------------------
650    # Monotonic toggle
651    # ------------------------------------------------------------------
652
653    def _on_monotonic_toggle(self):
654        """Handle the Monotonic checkbox toggle."""
655        self._push_undo()
656        self._monotonic = self._mono_var.get()
657        if self._monotonic:
658            self._enforce_monotonic()
659            self._draw()
660            self._status_var.set(
661                "Monotonic on — output increases with input")
662
663    # ------------------------------------------------------------------
664    # Buttons
665    # ------------------------------------------------------------------
666
667    def _on_reset(self):
668        self._push_undo()
669        self._points = default_segment_points()
670        if self._symmetric:
671            self._enforce_symmetry()
672        if self._monotonic:
673            self._enforce_monotonic()
674        self._draw()
675        self._status_var.set("Reset to linear (3 points)")
676
677    def _on_ok(self):
678        self._result = self._points
679        self.grab_release()
680        self.destroy()
681
682    def _on_cancel(self):
683        self._result = None
684        self.grab_release()
685        self.destroy()