mppt-testbench/testbench/gui.py

"""MPPT Testbench GUI -- live measurements + full instrument control.

Tkinter app with embedded matplotlib graphs and threaded instrument I/O.
"""

from __future__ import annotations

import csv
import queue
import sys
import threading
import time
import tkinter as tk
from tkinter import ttk, messagebox, filedialog
from collections import deque

import matplotlib
matplotlib.use("TkAgg")
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2Tk
from matplotlib.figure import Figure

from it6500.driver import IT6500
from prodigit3366g.driver import Prodigit3366G
from hioki3193.driver import Hioki3193
from testbench.bench import MPPTTestbench
from testbench.gui_workers import InstrumentWorker, Cmd


ERROR_THRESHOLD = 1e90
POLL_MS = 200  # GUI poll interval for checking worker data


def _fmt(val: float, decimals: int = 4) -> str:
    """Format a measurement value, showing '---' for error codes."""
    if abs(val) >= ERROR_THRESHOLD:
        return "---"
    return f"{val:+.{decimals}f}"


def _fmt_eng(val: float) -> str:
    """Format in engineering notation, showing '---' for error codes."""
    if abs(val) >= ERROR_THRESHOLD:
        return "---"
    return f"{val:+.4E}"


def _clean(val: float) -> float:
    """Replace HIOKI error codes with NaN for plotting."""
    return float("nan") if abs(val) >= ERROR_THRESHOLD else val


class _ConsoleRedirector:
    """Redirects stdout writes to the GUI console widget."""

    def __init__(self, gui: "TestbenchGUI") -> None:
        self._gui = gui
        self._orig = sys.stdout
        self._buf = ""

    def write(self, text: str) -> None:
        if self._orig:
            self._orig.write(text)
        # Buffer partial lines, flush on newline
        self._buf += text
        while "\n" in self._buf:
            line, self._buf = self._buf.split("\n", 1)
            line = line.strip()
            if line:
                self._gui._console(line)

    def flush(self) -> None:
        if self._orig:
            self._orig.flush()


class TestbenchGUI(tk.Tk):
    """Main GUI window."""

    def __init__(self) -> None:
        super().__init__()
        self.title("MPPT Testbench Control Panel")
        self.geometry("1500x950")
        self.minsize(1200, 800)

        self.bench: MPPTTestbench | None = None
        self.worker: InstrumentWorker | None = None
        self._log_file = None
        self._log_writer = None
        self._log_count = 0
        self._point_count = 0
        self._t0 = time.time()

        # Graph data
        self._history = 300
        self._timestamps: deque[float] = deque(maxlen=self._history)
        self._series: dict[str, deque[float]] = {
            k: deque(maxlen=self._history)
            for k in [
                "supply_P", "load_P",
                "meter_P5", "meter_P6", "meter_EFF1",
                "meter_U5", "meter_U6",
                "meter_I5", "meter_I6",
            ]
        }

        self._meter_fmt_sci = True  # True=scientific, False=normal
        self._sweep_data_queue: queue.Queue = queue.Queue(maxsize=100)

        self._build_ui()
        self.protocol("WM_DELETE_WINDOW", self._on_close)

    # ── UI Construction ───────────────────────────────────────────────

    def _build_ui(self) -> None:
        # Top bar
        self._build_connection_bar()
        # Main content: left controls + right graphs
        content = ttk.PanedWindow(self, orient=tk.HORIZONTAL)
        content.pack(fill=tk.BOTH, expand=True, padx=4, pady=4)
        # Left panel: two columns of controls
        left_outer = ttk.Frame(content)
        left_outer.columnconfigure(0, weight=1)
        left_outer.columnconfigure(1, weight=1)
        content.add(left_outer, weight=0)
        # Column 0: Supply + Load
        left_col0 = ttk.Frame(left_outer)
        left_col0.grid(row=0, column=0, sticky="nsew", padx=(0, 2))
        self._build_supply_controls(left_col0)
        self._build_load_controls(left_col0)
        self._build_logging_controls(left_col0)
        self._build_profile_controls(left_col0)
        # Column 1: Meter
        left_col1 = ttk.Frame(left_outer)
        left_col1.grid(row=0, column=1, sticky="nsew", padx=(2, 0))
        self._build_meter_readout(left_col1)
        self._build_sweep_vi_controls(left_col1)
        # Right panel: graphs + console
        right_pane = ttk.PanedWindow(content, orient=tk.VERTICAL)
        content.add(right_pane, weight=1)
        graph_frame = ttk.Frame(right_pane)
        right_pane.add(graph_frame, weight=3)
        self._build_graphs(graph_frame)
        console_frame = ttk.Frame(right_pane)
        right_pane.add(console_frame, weight=1)
        self._build_console(console_frame)
        # Status bar
        self._build_status_bar()

    def _build_connection_bar(self) -> None:
        bar = ttk.Frame(self)
        bar.pack(fill=tk.X, padx=4, pady=(4, 0))

        # Connection fields
        ttk.Label(bar, text="Supply:").pack(side=tk.LEFT)
        self._supply_addr = ttk.Entry(bar, width=20)
        self._supply_addr.insert(0, "auto")
        self._supply_addr.pack(side=tk.LEFT, padx=(2, 8))

        ttk.Label(bar, text="Load:").pack(side=tk.LEFT)
        self._load_port = ttk.Entry(bar, width=8)
        self._load_port.insert(0, "COM1")
        self._load_port.pack(side=tk.LEFT, padx=(2, 4))
        ttk.Label(bar, text="@").pack(side=tk.LEFT)
        self._load_baud = ttk.Entry(bar, width=7)
        self._load_baud.insert(0, "115200")
        self._load_baud.pack(side=tk.LEFT, padx=(2, 8))

        ttk.Label(bar, text="Meter:").pack(side=tk.LEFT)
        self._meter_addr = ttk.Entry(bar, width=20)
        self._meter_addr.insert(0, "auto")
        self._meter_addr.pack(side=tk.LEFT, padx=(2, 8))

        self._btn_connect = ttk.Button(bar, text="Connect", command=self._connect)
        self._btn_connect.pack(side=tk.LEFT, padx=2)
        self._btn_setup = ttk.Button(bar, text="Setup All", command=self._setup_all, state=tk.DISABLED)
        self._btn_setup.pack(side=tk.LEFT, padx=2)
        self._btn_disconnect = ttk.Button(bar, text="Disconnect", command=self._disconnect, state=tk.DISABLED)
        self._btn_disconnect.pack(side=tk.LEFT, padx=2)

        # SAFE OFF - always visible, right side
        self._btn_safe_off = tk.Button(
            bar, text="SAFE OFF", bg="#cc0000", fg="white",
            font=("TkDefaultFont", 13, "bold"), width=10,
            command=self._safe_off, activebackground="#ff0000",
        )
        self._btn_safe_off.pack(side=tk.RIGHT, padx=4)
        self.bind("<Escape>", lambda e: self._safe_off())

    def _build_supply_controls(self, parent) -> None:
        frame = ttk.LabelFrame(parent, text="DC Supply (IT6500D)", padding=8)
        frame.pack(fill=tk.X, padx=4, pady=4)

        # Setpoints
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        ttk.Label(row, text="Voltage (V):", width=12).pack(side=tk.LEFT)
        self._sup_voltage = ttk.Entry(row, width=10)
        self._sup_voltage.insert(0, "75.0")
        self._sup_voltage.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set V", width=6, command=self._set_supply_voltage).pack(side=tk.LEFT, padx=2)

        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        ttk.Label(row, text="Current (A):", width=12).pack(side=tk.LEFT)
        self._sup_current = ttk.Entry(row, width=10)
        self._sup_current.insert(0, "10.0")
        self._sup_current.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set I", width=6, command=self._set_supply_current).pack(side=tk.LEFT, padx=2)

        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        ttk.Button(row, text="Apply V+I", command=self._apply_supply).pack(side=tk.LEFT, padx=2)
        self._btn_sup_on = ttk.Button(row, text="Output ON", command=lambda: self._send(Cmd.OUTPUT_ON))
        self._btn_sup_on.pack(side=tk.LEFT, padx=2)
        self._btn_sup_off = ttk.Button(row, text="Output OFF", command=lambda: self._send(Cmd.OUTPUT_OFF))
        self._btn_sup_off.pack(side=tk.LEFT, padx=2)

        # OVP / Slew
        adv = ttk.LabelFrame(frame, text="Protection / Slew", padding=4)
        adv.pack(fill=tk.X, pady=4)
        row = ttk.Frame(adv)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="OVP (V):", width=12).pack(side=tk.LEFT)
        self._sup_ovp = ttk.Entry(row, width=10)
        self._sup_ovp.insert(0, "120.0")
        self._sup_ovp.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4, command=self._set_ovp).pack(side=tk.LEFT, padx=2)

        row = ttk.Frame(adv)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Rise (s):", width=12).pack(side=tk.LEFT)
        self._sup_rise = ttk.Entry(row, width=10)
        self._sup_rise.insert(0, "0.1")
        self._sup_rise.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send_float(Cmd.SET_RISE_TIME, self._sup_rise)).pack(side=tk.LEFT, padx=2)

        row = ttk.Frame(adv)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Fall (s):", width=12).pack(side=tk.LEFT)
        self._sup_fall = ttk.Entry(row, width=10)
        self._sup_fall.insert(0, "0.1")
        self._sup_fall.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send_float(Cmd.SET_FALL_TIME, self._sup_fall)).pack(side=tk.LEFT, padx=2)

        # Live readout
        readout = ttk.LabelFrame(frame, text="Measured", padding=4)
        readout.pack(fill=tk.X, pady=4)
        self._sup_v_label = ttk.Label(readout, text="V: ---", font=("Consolas", 11))
        self._sup_v_label.pack(anchor=tk.W)
        self._sup_i_label = ttk.Label(readout, text="I: ---", font=("Consolas", 11))
        self._sup_i_label.pack(anchor=tk.W)
        self._sup_p_label = ttk.Label(readout, text="P: ---", font=("Consolas", 11))
        self._sup_p_label.pack(anchor=tk.W)
        self._sup_state_label = tk.Label(
            readout, text="OUTPUT: ---", font=("Consolas", 11, "bold"),
            fg="#888888",
        )
        self._sup_state_label.pack(anchor=tk.W, pady=(4, 0))

    def _build_load_controls(self, parent) -> None:
        frame = ttk.LabelFrame(parent, text="DC Load (Prodigit 3366G)", padding=8)
        frame.pack(fill=tk.X, padx=4, pady=4)

        # Mode
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        ttk.Label(row, text="Mode:", width=12).pack(side=tk.LEFT)
        self._load_mode = ttk.Combobox(row, values=["CC", "CR", "CV", "CP"], width=6, state="readonly")
        self._load_mode.set("CC")
        self._load_mode.pack(side=tk.LEFT, padx=2)
        self._load_mode.bind("<<ComboboxSelected>>", self._on_mode_change)
        ttk.Button(row, text="Set Mode", command=self._set_load_mode).pack(side=tk.LEFT, padx=2)

        # Value
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        self._load_unit_label = ttk.Label(row, text="Value (A):", width=12)
        self._load_unit_label.pack(side=tk.LEFT)
        self._load_value = ttk.Entry(row, width=10)
        self._load_value.insert(0, "5.0")
        self._load_value.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=6, command=self._set_load_value).pack(side=tk.LEFT, padx=2)

        # On/Off
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        ttk.Button(row, text="Load ON", command=lambda: self._send(Cmd.LOAD_ON)).pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Load OFF", command=lambda: self._send(Cmd.LOAD_OFF)).pack(side=tk.LEFT, padx=2)

        # Slew rate
        adv = ttk.LabelFrame(frame, text="Slew Rate", padding=4)
        adv.pack(fill=tk.X, pady=4)
        row = ttk.Frame(adv)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Rise (A/us):", width=12).pack(side=tk.LEFT)
        self._load_rise = ttk.Entry(row, width=10)
        self._load_rise.insert(0, "1.0")
        self._load_rise.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send_float(Cmd.SET_SLEW_RISE, self._load_rise)).pack(side=tk.LEFT, padx=2)
        row = ttk.Frame(adv)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Fall (A/us):", width=12).pack(side=tk.LEFT)
        self._load_fall = ttk.Entry(row, width=10)
        self._load_fall.insert(0, "1.0")
        self._load_fall.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send_float(Cmd.SET_SLEW_FALL, self._load_fall)).pack(side=tk.LEFT, padx=2)

        # Live readout
        readout = ttk.LabelFrame(frame, text="Measured", padding=4)
        readout.pack(fill=tk.X, pady=4)
        self._load_v_label = ttk.Label(readout, text="V: ---", font=("Consolas", 11))
        self._load_v_label.pack(anchor=tk.W)
        self._load_i_label = ttk.Label(readout, text="I: ---", font=("Consolas", 11))
        self._load_i_label.pack(anchor=tk.W)
        self._load_p_label = ttk.Label(readout, text="P: ---", font=("Consolas", 11))
        self._load_p_label.pack(anchor=tk.W)
        self._load_state_label = tk.Label(
            readout, text="LOAD: ---", font=("Consolas", 11, "bold"),
            fg="#888888",
        )
        self._load_state_label.pack(anchor=tk.W, pady=(4, 0))

    def _build_meter_readout(self, parent) -> None:
        frame = ttk.LabelFrame(parent, text="Power Analyzer (HIOKI 3193-10)", padding=8)
        frame.pack(fill=tk.X, padx=4, pady=4)

        # Meter controls
        ctrl = ttk.LabelFrame(frame, text="Settings", padding=4)
        ctrl.pack(fill=tk.X, pady=4)

        row = ttk.Frame(ctrl)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Wiring:", width=10).pack(side=tk.LEFT)
        self._meter_wiring = ttk.Combobox(row, values=["1P2W", "1P3W", "3P3W", "3V3A", "3P4W"], width=6, state="readonly")
        self._meter_wiring.set("1P2W")
        self._meter_wiring.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send(Cmd.SET_WIRING, self._meter_wiring.get())).pack(side=tk.LEFT, padx=2)

        row = ttk.Frame(ctrl)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Speed:", width=10).pack(side=tk.LEFT)
        self._meter_speed = ttk.Combobox(row, values=["FAST", "MID", "SLOW"], width=6, state="readonly")
        self._meter_speed.set("SLOW")
        self._meter_speed.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send(Cmd.SET_RESPONSE_SPEED, self._meter_speed.get())).pack(side=tk.LEFT, padx=2)

        # Ch5 coupling
        row = ttk.Frame(ctrl)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Ch5 coup:", width=10).pack(side=tk.LEFT)
        self._ch5_coupling = ttk.Combobox(row, values=["DC", "AC", "ACDC"], width=6, state="readonly")
        self._ch5_coupling.set("DC")
        self._ch5_coupling.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send(Cmd.SET_COUPLING, 5, self._ch5_coupling.get())).pack(side=tk.LEFT, padx=2)

        # Ch6 coupling
        row = ttk.Frame(ctrl)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Ch6 coup:", width=10).pack(side=tk.LEFT)
        self._ch6_coupling = ttk.Combobox(row, values=["DC", "AC", "ACDC"], width=6, state="readonly")
        self._ch6_coupling.set("DC")
        self._ch6_coupling.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4,
                   command=lambda: self._send(Cmd.SET_COUPLING, 6, self._ch6_coupling.get())).pack(side=tk.LEFT, padx=2)

        # Format selector
        fmt_row = ttk.Frame(frame)
        fmt_row.pack(fill=tk.X, pady=2)
        ttk.Label(fmt_row, text="Format:", width=10).pack(side=tk.LEFT)
        self._meter_fmt_combo = ttk.Combobox(
            fmt_row, values=["Scientific", "Normal"], width=10, state="readonly"
        )
        self._meter_fmt_combo.set("Scientific")
        self._meter_fmt_combo.pack(side=tk.LEFT, padx=2)
        self._meter_fmt_combo.bind("<<ComboboxSelected>>", self._on_meter_fmt_change)

        # Voltage ranges: 6,15,30,60,150,300,600,1000
        v_ranges = ["AUTO", "6", "15", "30", "60", "150", "300", "600", "1000"]
        # Current ranges depend on clamp sensor; common values
        i_ranges = ["AUTO", "0.5", "1", "2", "5", "10", "20", "50", "100", "200", "500", "1000"]

        # Input side
        input_frame = ttk.LabelFrame(frame, text="Input (Ch5 - Solar)", padding=4)
        input_frame.pack(fill=tk.X, pady=2)
        self._m_u5 = ttk.Label(input_frame, text="U5: ---", font=("Consolas", 11))
        self._m_u5.pack(anchor=tk.W)
        self._m_i5 = ttk.Label(input_frame, text="I5:  ---", font=("Consolas", 11))
        self._m_i5.pack(anchor=tk.W)
        self._m_p5 = ttk.Label(input_frame, text="P5: ---", font=("Consolas", 11))
        self._m_p5.pack(anchor=tk.W)
        # Ch5 ranges
        rng5 = ttk.Frame(input_frame)
        rng5.pack(fill=tk.X, pady=(4, 0))
        ttk.Label(rng5, text="V range:").pack(side=tk.LEFT)
        self._ch5_v_range = ttk.Combobox(rng5, values=v_ranges, width=6, state="readonly")
        self._ch5_v_range.set("AUTO")
        self._ch5_v_range.pack(side=tk.LEFT, padx=2)
        ttk.Button(rng5, text="Set", width=3,
                   command=lambda: self._set_range(5, "V", self._ch5_v_range)).pack(side=tk.LEFT)
        ttk.Label(rng5, text="  I range:").pack(side=tk.LEFT)
        self._ch5_i_range = ttk.Combobox(rng5, values=i_ranges, width=6, state="readonly")
        self._ch5_i_range.set("AUTO")
        self._ch5_i_range.pack(side=tk.LEFT, padx=2)
        ttk.Button(rng5, text="Set", width=3,
                   command=lambda: self._set_range(5, "I", self._ch5_i_range)).pack(side=tk.LEFT)
        self._ch5_range_label = ttk.Label(input_frame, text="V: --- / I: ---", font=("Consolas", 9))
        self._ch5_range_label.pack(anchor=tk.W)

        # Output side
        output_frame = ttk.LabelFrame(frame, text="Output (Ch6 - MPPT)", padding=4)
        output_frame.pack(fill=tk.X, pady=2)
        self._m_u6 = ttk.Label(output_frame, text="U6: ---", font=("Consolas", 11))
        self._m_u6.pack(anchor=tk.W)
        self._m_i6 = ttk.Label(output_frame, text="I6:  ---", font=("Consolas", 11))
        self._m_i6.pack(anchor=tk.W)
        self._m_p6 = ttk.Label(output_frame, text="P6: ---", font=("Consolas", 11))
        self._m_p6.pack(anchor=tk.W)
        # Ch6 ranges
        rng6 = ttk.Frame(output_frame)
        rng6.pack(fill=tk.X, pady=(4, 0))
        ttk.Label(rng6, text="V range:").pack(side=tk.LEFT)
        self._ch6_v_range = ttk.Combobox(rng6, values=v_ranges, width=6, state="readonly")
        self._ch6_v_range.set("AUTO")
        self._ch6_v_range.pack(side=tk.LEFT, padx=2)
        ttk.Button(rng6, text="Set", width=3,
                   command=lambda: self._set_range(6, "V", self._ch6_v_range)).pack(side=tk.LEFT)
        ttk.Label(rng6, text="  I range:").pack(side=tk.LEFT)
        self._ch6_i_range = ttk.Combobox(rng6, values=i_ranges, width=6, state="readonly")
        self._ch6_i_range.set("AUTO")
        self._ch6_i_range.pack(side=tk.LEFT, padx=2)
        ttk.Button(rng6, text="Set", width=3,
                   command=lambda: self._set_range(6, "I", self._ch6_i_range)).pack(side=tk.LEFT)
        self._ch6_range_label = ttk.Label(output_frame, text="V: --- / I: ---", font=("Consolas", 9))
        self._ch6_range_label.pack(anchor=tk.W)

        # Efficiency - big and bold
        eff_frame = ttk.Frame(frame)
        eff_frame.pack(fill=tk.X, pady=4)
        self._m_eff = ttk.Label(eff_frame, text="EFF1: --- %", font=("Consolas", 16, "bold"))
        self._m_eff.pack(anchor=tk.CENTER)

    def _build_logging_controls(self, parent) -> None:
        frame = ttk.LabelFrame(parent, text="Data Logging", padding=8)
        frame.pack(fill=tk.X, padx=4, pady=4)

        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        ttk.Label(row, text="Interval (s):", width=12).pack(side=tk.LEFT)
        self._poll_interval = ttk.Entry(row, width=6)
        self._poll_interval.insert(0, "1.0")
        self._poll_interval.pack(side=tk.LEFT, padx=2)
        ttk.Button(row, text="Set", width=4, command=self._set_interval).pack(side=tk.LEFT, padx=2)

        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        self._btn_log_start = ttk.Button(row, text="Start Log", command=self._start_log)
        self._btn_log_start.pack(side=tk.LEFT, padx=2)
        self._btn_log_stop = ttk.Button(row, text="Stop Log", command=self._stop_log, state=tk.DISABLED)
        self._btn_log_stop.pack(side=tk.LEFT, padx=2)

        self._log_status = ttk.Label(frame, text="Not logging", font=("Consolas", 9))
        self._log_status.pack(anchor=tk.W, pady=2)

    def _build_profile_controls(self, parent) -> None:
        frame = ttk.LabelFrame(parent, text="Shade Profile", padding=8)
        frame.pack(fill=tk.X, padx=4, pady=4)

        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        self._profile_path_label = ttk.Label(row, text="No file selected", font=("Consolas", 9))
        self._profile_path_label.pack(side=tk.LEFT, fill=tk.X, expand=True)
        ttk.Button(row, text="Browse", command=self._browse_profile).pack(side=tk.RIGHT, padx=2)

        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        ttk.Label(row, text="Settle (s):", width=12).pack(side=tk.LEFT)
        self._profile_settle = ttk.Entry(row, width=6)
        self._profile_settle.insert(0, "2.0")
        self._profile_settle.pack(side=tk.LEFT, padx=2)

        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        self._btn_profile_run = ttk.Button(row, text="Run Profile", command=self._run_profile)
        self._btn_profile_run.pack(side=tk.LEFT, padx=2)
        self._btn_profile_stop = ttk.Button(row, text="Stop", command=self._stop_profile, state=tk.DISABLED)
        self._btn_profile_stop.pack(side=tk.LEFT, padx=2)

        self._profile_status = ttk.Label(frame, text="Idle", font=("Consolas", 9))
        self._profile_status.pack(anchor=tk.W, pady=2)

        self._profile_steps: list[dict] = []
        self._profile_index = 0
        self._profile_running = False
        self._profile_t0 = 0.0
        self._profile_after_id = None

    def _build_sweep_vi_controls(self, parent) -> None:
        frame = ttk.LabelFrame(parent, text="2D Sweep (V × Load)", padding=8)
        frame.pack(fill=tk.X, padx=4, pady=4)

        # Voltage range
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="V start:", width=10).pack(side=tk.LEFT)
        self._svi_v_start = ttk.Entry(row, width=7)
        self._svi_v_start.insert(0, "35")
        self._svi_v_start.pack(side=tk.LEFT, padx=2)
        ttk.Label(row, text="stop:").pack(side=tk.LEFT)
        self._svi_v_stop = ttk.Entry(row, width=7)
        self._svi_v_stop.insert(0, "100")
        self._svi_v_stop.pack(side=tk.LEFT, padx=2)
        ttk.Label(row, text="step:").pack(side=tk.LEFT)
        self._svi_v_step = ttk.Entry(row, width=5)
        self._svi_v_step.insert(0, "5")
        self._svi_v_step.pack(side=tk.LEFT, padx=2)

        # Load mode selector
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="Load mode:", width=10).pack(side=tk.LEFT)
        self._svi_load_mode = ttk.Combobox(row, values=["CC", "CP"], width=4, state="readonly")
        self._svi_load_mode.set("CC")
        self._svi_load_mode.pack(side=tk.LEFT, padx=2)
        self._svi_load_mode.bind("<<ComboboxSelected>>", self._on_svi_mode_change)

        # Load setpoint range
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=1)
        self._svi_l_label = ttk.Label(row, text="I start:", width=10)
        self._svi_l_label.pack(side=tk.LEFT)
        self._svi_l_start = ttk.Entry(row, width=7)
        self._svi_l_start.insert(0, "0.5")
        self._svi_l_start.pack(side=tk.LEFT, padx=2)
        ttk.Label(row, text="stop:").pack(side=tk.LEFT)
        self._svi_l_stop = ttk.Entry(row, width=7)
        self._svi_l_stop.insert(0, "30")
        self._svi_l_stop.pack(side=tk.LEFT, padx=2)
        ttk.Label(row, text="step:").pack(side=tk.LEFT)
        self._svi_l_step = ttk.Entry(row, width=5)
        self._svi_l_step.insert(0, "1")
        self._svi_l_step.pack(side=tk.LEFT, padx=2)

        # Supply current limit + settle
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=1)
        ttk.Label(row, text="I limit:", width=10).pack(side=tk.LEFT)
        self._svi_ilimit = ttk.Entry(row, width=7)
        self._svi_ilimit.insert(0, "35")
        self._svi_ilimit.pack(side=tk.LEFT, padx=2)
        ttk.Label(row, text="settle:").pack(side=tk.LEFT)
        self._svi_settle = ttk.Entry(row, width=5)
        self._svi_settle.insert(0, "2.0")
        self._svi_settle.pack(side=tk.LEFT, padx=2)
        ttk.Label(row, text="s").pack(side=tk.LEFT)

        # Run / Stop
        row = ttk.Frame(frame)
        row.pack(fill=tk.X, pady=2)
        self._btn_svi_run = ttk.Button(row, text="Run Sweep", command=self._run_sweep_vi)
        self._btn_svi_run.pack(side=tk.LEFT, padx=2)
        self._btn_svi_stop = ttk.Button(row, text="Stop", command=self._stop_sweep_vi, state=tk.DISABLED)
        self._btn_svi_stop.pack(side=tk.LEFT, padx=2)

        self._svi_status = ttk.Label(frame, text="Idle", font=("Consolas", 9))
        self._svi_status.pack(anchor=tk.W, pady=2)

        self._svi_thread = None
        self._svi_stop_event = None

    def _build_graphs(self, parent) -> None:
        self._fig = Figure(figsize=(10, 8), dpi=100)
        self._fig.suptitle("MPPT Testbench Live", fontsize=12, fontweight="bold")

        self._ax_power = self._fig.add_subplot(4, 1, 1)
        self._ax_eff = self._fig.add_subplot(4, 1, 2)
        self._ax_volt = self._fig.add_subplot(4, 1, 3)
        self._ax_curr = self._fig.add_subplot(4, 1, 4)

        # Power
        self._ax_power.set_ylabel("Power (W)")
        self._ax_power.set_title("Power", fontsize=10)
        self._ax_power.grid(True, alpha=0.3)
        self._ln_p5, = self._ax_power.plot([], [], label="P_in (meter)", linewidth=1.5)
        self._ln_p6, = self._ax_power.plot([], [], label="P_out (meter)", linewidth=1.5)
        self._ln_sp, = self._ax_power.plot([], [], label="Supply P", linewidth=1, ls="--", alpha=0.6)
        self._ln_lp, = self._ax_power.plot([], [], label="Load P", linewidth=1, ls="--", alpha=0.6)
        self._ax_power.legend(loc="upper left", fontsize=8)

        # Efficiency
        self._ax_eff.set_ylabel("Efficiency (%)")
        self._ax_eff.set_title("Efficiency", fontsize=10)
        self._ax_eff.grid(True, alpha=0.3)
        self._ln_eff, = self._ax_eff.plot([], [], label="EFF1", linewidth=1.5, color="green")
        self._ax_eff.legend(loc="upper left", fontsize=8)

        # Voltage
        self._ax_volt.set_ylabel("Voltage (V)")
        self._ax_volt.set_title("Voltage", fontsize=10)
        self._ax_volt.grid(True, alpha=0.3)
        self._ln_u5, = self._ax_volt.plot([], [], label="U5 (input)", linewidth=1.5)
        self._ln_u6, = self._ax_volt.plot([], [], label="U6 (output)", linewidth=1.5)
        self._ax_volt.legend(loc="upper left", fontsize=8)

        # Current
        self._ax_curr.set_ylabel("Current (A)")
        self._ax_curr.set_xlabel("Time (s)")
        self._ax_curr.set_title("Current", fontsize=10)
        self._ax_curr.grid(True, alpha=0.3)
        self._ln_i5, = self._ax_curr.plot([], [], label="I5 (input)", linewidth=1.5)
        self._ln_i6, = self._ax_curr.plot([], [], label="I6 (output)", linewidth=1.5)
        self._ax_curr.legend(loc="upper left", fontsize=8)

        self._fig.tight_layout()

        self._canvas = FigureCanvasTkAgg(self._fig, master=parent)
        self._canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
        toolbar = NavigationToolbar2Tk(self._canvas, parent)
        toolbar.update()

    def _build_console(self, parent) -> None:
        frame = ttk.LabelFrame(parent, text="Console", padding=4)
        frame.pack(fill=tk.BOTH, expand=True, padx=0, pady=0)

        self._console_text = tk.Text(
            frame, height=8, font=("Consolas", 9),
            wrap=tk.WORD, state=tk.DISABLED,
            bg="#1e1e1e", fg="#cccccc", insertbackground="#cccccc",
        )
        scrollbar = ttk.Scrollbar(frame, orient=tk.VERTICAL, command=self._console_text.yview)
        self._console_text.config(yscrollcommand=scrollbar.set)
        scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
        self._console_text.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)

        # Tag for error messages
        self._console_text.tag_configure("error", foreground="#ff6b6b")
        self._console_text.tag_configure("success", foreground="#69db7c")
        self._console_text.tag_configure("warn", foreground="#ffd43b")

        # Redirect stdout to console
        self._orig_stdout = sys.stdout
        sys.stdout = _ConsoleRedirector(self)

    def _console(self, msg: str, tag: str = "") -> None:
        """Append a message to the console log. Thread-safe."""
        ts = time.strftime("%H:%M:%S")
        line = f"[{ts}] {msg}\n"

        def _append():
            self._console_text.config(state=tk.NORMAL)
            if tag:
                self._console_text.insert(tk.END, line, tag)
            else:
                self._console_text.insert(tk.END, line)
            self._console_text.see(tk.END)
            self._console_text.config(state=tk.DISABLED)

        # If called from a non-main thread, schedule on main thread
        try:
            if threading.current_thread() is threading.main_thread():
                _append()
            else:
                self.after(0, _append)
        except RuntimeError:
            pass

    def _build_status_bar(self) -> None:
        bar = ttk.Frame(self)
        bar.pack(fill=tk.X, padx=4, pady=(0, 4))
        self._status_label = ttk.Label(bar, text="Disconnected", font=("Consolas", 9))
        self._status_label.pack(side=tk.LEFT)

    # ── Connection ────────────────────────────────────────────────────

    def _connect(self) -> None:
        try:
            supply_addr = self._supply_addr.get().strip()
            if supply_addr == "auto":
                supply_addr = MPPTTestbench.find_supply(None)
            meter_addr = self._meter_addr.get().strip()
            if meter_addr == "auto":
                meter_addr = MPPTTestbench.find_meter(None)
            load_port = self._load_port.get().strip()
            load_baud = int(self._load_baud.get().strip())

            supply = IT6500(supply_addr)
            load = Prodigit3366G(load_port, baudrate=load_baud)
            meter = Hioki3193(meter_addr)

            self.bench = MPPTTestbench(supply, load, meter)
            self.bench.supply.remote()
            self.bench.load.remote()

            self.worker = InstrumentWorker(self.bench, interval=1.0)
            self.worker.start()

            self._t0 = time.time()
            self._point_count = 0

            # Update UI state
            self._btn_connect.config(state=tk.DISABLED)
            self._btn_setup.config(state=tk.NORMAL)
            self._btn_disconnect.config(state=tk.NORMAL)
            self._supply_addr.config(state=tk.DISABLED)
            self._load_port.config(state=tk.DISABLED)
            self._load_baud.config(state=tk.DISABLED)
            self._meter_addr.config(state=tk.DISABLED)

            self._status_label.config(text="Connected")
            self._console(f"Connected: supply={supply_addr}, load={load_port}, meter={meter_addr}", "success")
            self._poll()

        except Exception as e:
            self._console(f"Connection failed: {e}", "error")
            messagebox.showerror("Connection Error", str(e))

    def _disconnect(self) -> None:
        self._stop_log()
        if self.worker:
            self.worker.stop()
            self.worker = None
        if self.bench:
            try:
                self.bench.close()
            except Exception:
                pass
            self.bench = None

        self._btn_connect.config(state=tk.NORMAL)
        self._btn_setup.config(state=tk.DISABLED)
        self._btn_disconnect.config(state=tk.DISABLED)
        self._supply_addr.config(state=tk.NORMAL)
        self._load_port.config(state=tk.NORMAL)
        self._load_baud.config(state=tk.NORMAL)
        self._meter_addr.config(state=tk.NORMAL)
        self._status_label.config(text="Disconnected")
        self._console("Disconnected")

    def _setup_all(self) -> None:
        self._send(Cmd.SETUP_ALL)
        self._console("Setup All sent")

    def _safe_off(self) -> None:
        """Emergency stop -- bypasses command queue for minimum latency."""
        self._console("SAFE OFF triggered", "error")
        if self.bench:
            try:
                self.bench.safe_off()
            except Exception:
                pass
        if self.worker:
            self.worker.send(Cmd.SAFE_OFF)

    def _on_close(self) -> None:
        self._stop_log()
        if self.bench:
            try:
                self.bench.safe_off()
            except Exception:
                pass
        self._disconnect()
        sys.stdout = self._orig_stdout
        self.destroy()

    # ── Polling / Data Update ─────────────────────────────────────────

    def _poll(self) -> None:
        """Called periodically to pull data from the worker and update UI."""
        data = None

        if self.worker:
            data = self.worker.get_data()
        else:
            # During 2D sweep the worker is stopped; drain sweep queue
            latest = None
            while True:
                try:
                    latest = self._sweep_data_queue.get_nowait()
                except queue.Empty:
                    break
            data = latest

        if data:
            error = data.get("_error")
            if error:
                self._console(f"Error: {error}", "error")
                self._status_label.config(text=f"Error: {error}")
            else:
                self._update_readouts(data)
                self._update_graphs(data)
                self._log_data(data)
                self._point_count += 1

                elapsed = time.time() - self._t0
                mins, secs = divmod(int(elapsed), 60)
                hrs, mins = divmod(mins, 60)
                self._status_label.config(
                    text=f"Connected | {self._point_count} pts | {hrs:02d}:{mins:02d}:{secs:02d}"
                )

        # Keep polling as long as connected (bench exists)
        if self.worker or self.bench:
            self.after(POLL_MS, self._poll)

    def _update_readouts(self, data: dict) -> None:
        """Update all numeric labels from measurement data."""
        # Supply
        self._sup_v_label.config(text=f"V: {_fmt(data['supply_V'])} V")
        self._sup_i_label.config(text=f"I: {_fmt(data['supply_I'])} A")
        self._sup_p_label.config(text=f"P: {_fmt(data['supply_P'])} W")

        # Load
        self._load_v_label.config(text=f"V: {_fmt(data['load_V'])} V")
        self._load_i_label.config(text=f"I: {_fmt(data['load_I'])} A")
        self._load_p_label.config(text=f"P: {_fmt(data['load_P'])} W")

        # Meter
        fm = self._fmt_meter
        self._m_u5.config(text=f"U5: {fm(data['meter_U5'])} V")
        self._m_i5.config(text=f"I5:  {fm(data['meter_I5'])} A")
        self._m_p5.config(text=f"P5: {fm(data['meter_P5'])} W")
        self._m_u6.config(text=f"U6: {fm(data['meter_U6'])} V")
        self._m_i6.config(text=f"I6:  {fm(data['meter_I6'])} A")
        self._m_p6.config(text=f"P6: {fm(data['meter_P6'])} W")

        eff = data['meter_EFF1']
        if abs(eff) >= ERROR_THRESHOLD:
            self._m_eff.config(text="EFF1: --- %")
        else:
            self._m_eff.config(text=f"EFF1: {eff:.2f} %")

        # ON/OFF indicators
        supply_on = data.get("supply_on")
        if supply_on is True:
            self._sup_state_label.config(text="OUTPUT: ON", fg="#00aa00")
        elif supply_on is False:
            self._sup_state_label.config(text="OUTPUT: OFF", fg="#cc0000")
        else:
            self._sup_state_label.config(text="OUTPUT: ---", fg="#888888")

        load_on = data.get("load_on")
        if load_on is True:
            self._load_state_label.config(text="LOAD: ON", fg="#00aa00")
        elif load_on is False:
            self._load_state_label.config(text="LOAD: OFF", fg="#cc0000")
        else:
            self._load_state_label.config(text="LOAD: ---", fg="#888888")

        # Meter channel ranges
        vr5 = data.get("v_range_5")
        ir5 = data.get("i_range_5")
        self._ch5_range_label.config(
            text=f"V: {vr5 or '---'} / I: {ir5 or '---'}"
        )
        vr6 = data.get("v_range_6")
        ir6 = data.get("i_range_6")
        self._ch6_range_label.config(
            text=f"V: {vr6 or '---'} / I: {ir6 or '---'}"
        )

    def _update_graphs(self, data: dict) -> None:
        """Append data to series and redraw graphs."""
        now = time.time() - self._t0
        self._timestamps.append(now)

        for key in self._series:
            self._series[key].append(_clean(data.get(key, 0.0)))

        t = list(self._timestamps)
        self._ln_p5.set_data(t, list(self._series["meter_P5"]))
        self._ln_p6.set_data(t, list(self._series["meter_P6"]))
        self._ln_sp.set_data(t, list(self._series["supply_P"]))
        self._ln_lp.set_data(t, list(self._series["load_P"]))
        self._ln_eff.set_data(t, list(self._series["meter_EFF1"]))
        self._ln_u5.set_data(t, list(self._series["meter_U5"]))
        self._ln_u6.set_data(t, list(self._series["meter_U6"]))
        self._ln_i5.set_data(t, list(self._series["meter_I5"]))
        self._ln_i6.set_data(t, list(self._series["meter_I6"]))

        for ax in [self._ax_power, self._ax_eff, self._ax_volt, self._ax_curr]:
            ax.relim()
            ax.autoscale_view()

        self._canvas.draw_idle()

    # ── Format Helpers ─────────────────────────────────────────────────

    def _on_meter_fmt_change(self, _event=None) -> None:
        self._meter_fmt_sci = self._meter_fmt_combo.get() == "Scientific"

    def _fmt_meter(self, val: float) -> str:
        """Format a meter value based on the current format selection."""
        if self._meter_fmt_sci:
            return _fmt_eng(val)
        return _fmt(val, decimals=4)

    def _on_svi_mode_change(self, _event=None) -> None:
        mode = self._svi_load_mode.get()
        if mode == "CC":
            self._svi_l_label.config(text="I start:")
        else:
            self._svi_l_label.config(text="P start:")

    def _set_range(self, channel: int, vi: str, combo: ttk.Combobox) -> None:
        """Set voltage or current range for a HIOKI channel."""
        val = combo.get()
        if val == "AUTO":
            if vi == "V":
                self._send(Cmd.SET_VOLTAGE_AUTO, channel, True)
            else:
                self._send(Cmd.SET_CURRENT_AUTO, channel, True)
        else:
            if vi == "V":
                self._send(Cmd.SET_VOLTAGE_RANGE, channel, int(val))
            else:
                self._send(Cmd.SET_CURRENT_RANGE, channel, float(val))

    # ── Command Helpers ───────────────────────────────────────────────

    def _send(self, cmd: Cmd, *args) -> None:
        """Send a command to the worker thread."""
        if self.worker:
            self.worker.send(cmd, *args)
            if args:
                self._console(f"{cmd.name} {' '.join(str(a) for a in args)}")
            else:
                self._console(cmd.name)

    def _send_float(self, cmd: Cmd, entry: ttk.Entry) -> None:
        """Parse an entry as float and send to worker."""
        try:
            val = float(entry.get())
            self._send(cmd, val)
        except ValueError:
            entry.config(foreground="red")
            self.after(1000, lambda: entry.config(foreground=""))

    def _set_supply_voltage(self) -> None:
        self._send_float(Cmd.SET_VOLTAGE, self._sup_voltage)

    def _set_supply_current(self) -> None:
        self._send_float(Cmd.SET_CURRENT, self._sup_current)

    def _apply_supply(self) -> None:
        try:
            v = float(self._sup_voltage.get())
            i = float(self._sup_current.get())
            self._send(Cmd.APPLY, v, i)
        except ValueError:
            pass

    def _set_ovp(self) -> None:
        self._send_float(Cmd.SET_OVP, self._sup_ovp)

    def _set_load_mode(self) -> None:
        self._send(Cmd.SET_MODE, self._load_mode.get())

    def _set_load_value(self) -> None:
        try:
            val = float(self._load_value.get())
            mode = self._load_mode.get()
            self._send(Cmd.SET_MODE_VALUE, mode, val)
        except ValueError:
            self._load_value.config(foreground="red")
            self.after(1000, lambda: self._load_value.config(foreground=""))

    def _on_mode_change(self, _event=None) -> None:
        """Update the value label units when load mode changes."""
        units = {"CC": "A", "CR": "\u03a9", "CV": "V", "CP": "W"}
        mode = self._load_mode.get()
        self._load_unit_label.config(text=f"Value ({units.get(mode, '?')}):")

    def _set_interval(self) -> None:
        try:
            val = float(self._poll_interval.get())
            if val < 0.2:
                val = 0.2
            self._send(Cmd.SET_INTERVAL, val)
        except ValueError:
            pass

    # ── Shade Profile ──────────────────────────────────────────────────

    def _browse_profile(self) -> None:
        path = filedialog.askopenfilename(
            filetypes=[("CSV files", "*.csv"), ("All files", "*.*")],
        )
        if not path:
            return
        try:
            self._profile_steps = MPPTTestbench.load_shade_profile(path)
            n = len(self._profile_steps)
            dur = self._profile_steps[-1]["time"] if self._profile_steps else 0
            self._profile_path_label.config(text=path.split("/")[-1].split("\\")[-1])
            self._profile_status.config(text=f"Loaded: {n} steps, {dur:.0f}s")
        except Exception as e:
            messagebox.showerror("Profile Error", str(e))

    def _run_profile(self) -> None:
        if not self._profile_steps:
            messagebox.showwarning("No Profile", "Load a profile CSV first.")
            return
        if not self.worker:
            return

        try:
            settle = float(self._profile_settle.get())
        except ValueError:
            settle = 2.0

        n = len(self._profile_steps)
        dur = self._profile_steps[-1]["time"] if self._profile_steps else 0
        self._console(f"Shade profile started: {n} steps, {dur:.0f}s", "success")

        self._profile_running = True
        self._profile_index = 0
        self._profile_t0 = time.time()
        self._btn_profile_run.config(state=tk.DISABLED)
        self._btn_profile_stop.config(state=tk.NORMAL)

        # Turn outputs on with first step
        first = self._profile_steps[0]
        self._send(Cmd.SET_CURRENT, first["current_limit"])
        self._send(Cmd.SET_VOLTAGE, first["voltage"])
        self._send(Cmd.OUTPUT_ON)
        if "load_mode" in first:
            self._send(Cmd.SET_MODE, first["load_mode"])
        if "load_value" in first:
            self._send(Cmd.SET_MODE_VALUE, first.get("load_mode", "CC"), first["load_value"])
        self._send(Cmd.LOAD_ON)

        self._profile_schedule_next()

    def _profile_schedule_next(self) -> None:
        if not self._profile_running:
            return
        if self._profile_index >= len(self._profile_steps):
            self._finish_profile()
            return

        step = self._profile_steps[self._profile_index]
        target = self._profile_t0 + step["time"]
        delay_ms = max(0, int((target - time.time()) * 1000))

        self._profile_after_id = self.after(delay_ms, self._apply_profile_step)

    def _apply_profile_step(self) -> None:
        if not self._profile_running:
            return

        step = self._profile_steps[self._profile_index]
        self._send(Cmd.SET_CURRENT, step["current_limit"])
        self._send(Cmd.SET_VOLTAGE, step["voltage"])
        if "load_mode" in step:
            self._send(Cmd.SET_MODE, step["load_mode"])
        if "load_value" in step:
            self._send(Cmd.SET_MODE_VALUE, step.get("load_mode", "CC"), step["load_value"])

        elapsed = time.time() - self._profile_t0
        n = len(self._profile_steps)
        self._profile_status.config(
            text=f"Step {self._profile_index + 1}/{n}  "
                 f"t={elapsed:.0f}s  V={step['voltage']:.1f}V  "
                 f"I_lim={step['current_limit']:.1f}A"
        )

        self._profile_index += 1
        self._profile_schedule_next()

    def _stop_profile(self) -> None:
        self._profile_running = False
        if self._profile_after_id:
            self.after_cancel(self._profile_after_id)
            self._profile_after_id = None
        self._btn_profile_run.config(state=tk.NORMAL)
        self._btn_profile_stop.config(state=tk.DISABLED)
        self._profile_status.config(text="Stopped")
        self._console("Shade profile stopped", "warn")

    def _finish_profile(self) -> None:
        self._profile_running = False
        self._btn_profile_run.config(state=tk.NORMAL)
        self._btn_profile_stop.config(state=tk.DISABLED)
        elapsed = time.time() - self._profile_t0
        self._profile_status.config(text=f"Done ({elapsed:.0f}s)")
        self._console(f"Shade profile complete ({elapsed:.0f}s)", "success")

    # ── 2D Sweep (V × I) ───────────────────────────────────────────────

    def _run_sweep_vi(self) -> None:
        if not self.bench:
            return
        try:
            params = {
                "v_start": float(self._svi_v_start.get()),
                "v_stop": float(self._svi_v_stop.get()),
                "v_step": float(self._svi_v_step.get()),
                "l_start": float(self._svi_l_start.get()),
                "l_stop": float(self._svi_l_stop.get()),
                "l_step": float(self._svi_l_step.get()),
                "current_limit": float(self._svi_ilimit.get()),
                "settle_time": float(self._svi_settle.get()),
                "load_mode": self._svi_load_mode.get(),
            }
        except ValueError:
            messagebox.showerror("Invalid Input", "Check sweep parameters.")
            return

        # Ask for output file
        default_name = time.strftime("sweep_vi_%Y%m%d_%H%M%S.csv")
        path = filedialog.asksaveasfilename(
            defaultextension=".csv",
            filetypes=[("CSV files", "*.csv")],
            initialfile=default_name,
        )
        if not path:
            return

        mode = params["load_mode"]
        unit = "A" if mode == "CC" else "W"
        self._console(
            f"2D sweep: V={params['v_start']}-{params['v_stop']}V "
            f"{mode}={params['l_start']}-{params['l_stop']}{unit}",
            "success",
        )

        # Stop the normal worker so the sweep owns the instruments
        if self.worker:
            self.worker.stop()
            self.worker = None

        # Restart _poll so it drains the sweep data queue
        self.after(POLL_MS, self._poll)

        self._btn_svi_run.config(state=tk.DISABLED)
        self._btn_svi_stop.config(state=tk.NORMAL)

        stop_event = threading.Event()
        self._svi_stop_event = stop_event

        def _sweep_thread():
            try:
                self.after(0, lambda: self._svi_status.config(text="Running..."))
                results = self._sweep_vi_loop(params, stop_event)
                # Write CSV
                if results:
                    self._write_sweep_vi_csv(results, path)
                    fname = path.split('/')[-1].split(chr(92))[-1]
                    msg = f"Done: {len(results)} pts saved to {fname}"
                    self._console(msg, "success")
                else:
                    msg = "Sweep stopped (no data)"
                    self._console(msg, "warn")
                self.after(0, lambda: self._svi_status.config(text=msg))
            except Exception as e:
                err_msg = f"Error: {e}"
                self._console(f"Sweep error: {e}", "error")
                self.after(0, lambda: self._svi_status.config(text=err_msg))
            finally:
                self.after(0, self._sweep_vi_done)

        self._svi_thread = threading.Thread(target=_sweep_thread, daemon=True)
        self._svi_thread.start()

    def _sweep_vi_loop(self, p: dict, stop: threading.Event) -> list:
        """Run the 2D sweep on a background thread. Returns list of SweepPoint."""
        from testbench.bench import IDLE_VOLTAGE

        bench = self.bench
        v_start, v_stop, v_step = p["v_start"], p["v_stop"], p["v_step"]
        l_start, l_stop, l_step = p["l_start"], p["l_stop"], p["l_step"]
        current_limit = p["current_limit"]
        settle = p["settle_time"]
        load_mode = p.get("load_mode", "CC")
        unit = "A" if load_mode == "CC" else "W"

        # Auto-correct directions
        if v_start < v_stop and v_step < 0:
            v_step = -v_step
        elif v_start > v_stop and v_step > 0:
            v_step = -v_step
        if l_start < l_stop and l_step < 0:
            l_step = -l_step
        elif l_start > l_stop and l_step > 0:
            l_step = -l_step

        # Sanity check
        max_v = max(abs(v_start), abs(v_stop))
        min_v = min(abs(v_start), abs(v_stop))
        max_l = max(abs(l_start), abs(l_stop))
        bench.check_supply_capability(
            max_v, current_limit,
            min_voltage=min_v,
            load_mode=load_mode,
            max_load_setpoint=max_l,
        )

        bench.supply.set_current(current_limit)
        bench.supply.output_on()
        bench.load.set_mode(load_mode)
        bench._apply_load_value(load_mode, l_start)
        bench.load.load_on()

        results = []
        n = 0
        v = v_start

        try:
            while not stop.is_set():
                if v_step > 0 and v > v_stop + v_step / 2:
                    break
                if v_step < 0 and v < v_stop + v_step / 2:
                    break

                bench.supply.set_voltage(v)
                ll = l_start

                while not stop.is_set():
                    if l_step > 0 and ll > l_stop + l_step / 2:
                        break
                    if l_step < 0 and ll < l_stop + l_step / 2:
                        break

                    bench._apply_load_value(load_mode, ll)
                    time.sleep(settle)
                    if stop.is_set():
                        break

                    point = bench._record_point(v, current_limit, load_setpoint=ll)
                    results.append(point)
                    n += 1

                    # Push data for live graph/readout updates
                    gui_data = {
                        "supply_V": point.supply_voltage,
                        "supply_I": point.supply_current,
                        "supply_P": point.supply_power,
                        "load_V": point.load_voltage,
                        "load_I": point.load_current,
                        "load_P": point.load_power,
                        "meter_U5": point.supply_voltage,
                        "meter_I5": point.supply_current,
                        "meter_P5": point.input_power,
                        "meter_U6": point.load_voltage,
                        "meter_I6": point.load_current,
                        "meter_P6": point.output_power,
                        "meter_EFF1": point.efficiency,
                        "supply_on": True,
                        "load_on": True,
                        "_error": None,
                        "_timestamp": time.time(),
                    }
                    try:
                        self._sweep_data_queue.put_nowait(gui_data)
                    except queue.Full:
                        try:
                            self._sweep_data_queue.get_nowait()
                        except queue.Empty:
                            pass
                        self._sweep_data_queue.put_nowait(gui_data)

                    self.after(
                        0,
                        lambda v=v, ll=ll, pt=point, n=n: self._svi_status.config(
                            text=f"[{n}] V={v:.1f}V {load_mode}={ll:.1f}{unit} "
                                 f"EFF={pt.efficiency:.1f}%"
                        ),
                    )

                    ll += l_step
                v += v_step
        finally:
            bench.load.load_off()
            bench.supply.set_voltage(IDLE_VOLTAGE)

        return results

    @staticmethod
    def _write_sweep_vi_csv(results, path: str) -> None:
        import csv as _csv

        with open(path, "w", newline="") as f:
            w = _csv.writer(f)
            w.writerow([
                "voltage_set", "current_limit", "load_setpoint",
                "supply_V", "supply_I", "supply_P",
                "load_V", "load_I", "load_P",
                "input_power", "output_power", "efficiency",
            ])
            for pt in results:
                w.writerow([
                    f"{pt.voltage_set:.4f}", f"{pt.current_limit:.4f}",
                    f"{pt.load_setpoint:.4f}",
                    f"{pt.supply_voltage:.4f}", f"{pt.supply_current:.4f}",
                    f"{pt.supply_power:.4f}",
                    f"{pt.load_voltage:.4f}", f"{pt.load_current:.4f}",
                    f"{pt.load_power:.4f}",
                    f"{pt.input_power:.4f}", f"{pt.output_power:.4f}",
                    f"{pt.efficiency:.4f}",
                ])

    def _stop_sweep_vi(self) -> None:
        if self._svi_stop_event:
            self._svi_stop_event.set()

    def _sweep_vi_done(self) -> None:
        self._btn_svi_run.config(state=tk.NORMAL)
        self._btn_svi_stop.config(state=tk.DISABLED)
        self._svi_thread = None
        self._svi_stop_event = None
        # Restart normal worker polling
        if self.bench:
            interval = 1.0
            try:
                interval = float(self._poll_interval.get())
            except ValueError:
                pass
            self.worker = InstrumentWorker(self.bench, interval=interval)
            self.worker.start()
            self._poll()

    # ── Logging ───────────────────────────────────────────────────────

    _LOG_COLUMNS = [
        "timestamp",
        "supply_V", "supply_I", "supply_P",
        "load_V", "load_I", "load_P",
        "meter_U5", "meter_I5", "meter_P5",
        "meter_U6", "meter_I6", "meter_P6",
        "meter_EFF1",
    ]

    def _start_log(self) -> None:
        default_name = time.strftime("data_%Y%m%d_%H%M%S.csv")
        path = filedialog.asksaveasfilename(
            defaultextension=".csv",
            filetypes=[("CSV files", "*.csv")],
            initialfile=default_name,
        )
        if not path:
            return
        self._log_file = open(path, "w", newline="")
        self._log_writer = csv.writer(self._log_file)
        self._log_writer.writerow(self._LOG_COLUMNS)
        self._log_count = 0
        self._btn_log_start.config(state=tk.DISABLED)
        self._btn_log_stop.config(state=tk.NORMAL)
        self._log_status.config(text=f"Logging to {path}")
        self._console(f"CSV logging started: {path}")

    def _stop_log(self) -> None:
        if self._log_file:
            self._console(f"CSV logging stopped ({self._log_count} samples)")
            self._log_file.close()
            self._log_file = None
            self._log_writer = None
        self._btn_log_start.config(state=tk.NORMAL)
        self._btn_log_stop.config(state=tk.DISABLED)
        self._log_status.config(text=f"Stopped ({self._log_count} samples)")

    def _log_data(self, data: dict) -> None:
        if not self._log_writer:
            return
        row = [time.strftime("%Y-%m-%d %H:%M:%S")]
        for col in self._LOG_COLUMNS[1:]:
            row.append(f"{data.get(col, 0.0):.6f}")
        self._log_writer.writerow(row)
        self._log_file.flush()
        self._log_count += 1
        self._log_status.config(text=f"Logging... {self._log_count} samples")


def main() -> None:
    app = TestbenchGUI()
    app.mainloop()


if __name__ == "__main__":
    main()