dc_motor_gui.py

# Small GUI tool to trigger an ESP32 experiment over a serial port and save the
# resulting stream of measurements to a CSV file.
#
# High-level behavior:
# - Opens a serial port to an ESP32 and optionally pulses DTR to reset the board.
# - Starts a background reader thread that reads raw bytes, splits into text lines
#   and hands complete lines to a handler.
# - The handler posts log lines to a queue for the main (GUI) thread, tracks a
#   READY handshake, parses semicolon-separated data rows while an experiment is
#   "collecting", and reacts to an "END" marker by saving the collected data.
# - The main thread periodically drains the queue and appends lines to the log
#   widget; saving to CSV is performed on the main thread (GUI context).
#
# Threading / synchronization notes:
# - The reader thread is a daemon that performs non-blocking reads and sleeps
#   briefly when no data is available to avoid busy looping.
# - A queue.Queue is used to transfer text/log messages from the reader thread
#   to the GUI thread (Tkinter must only be touched on the main thread).
# - A threading.Lock protects the in-memory list of parsed data rows so both
#   threads may safely access it (reader appends, main thread reads/writes when
#   saving or starting a new experiment).
#
# Serial robustness:
# - Reads are performed into a local byte buffer; lines are extracted at LF
#   boundaries to handle partial reads and different newline conventions.
# - Decoding uses errors='ignore' to be tolerant of transient garbage.
# - Opening the serial port is guarded with exceptions and reports errors via
#   message boxes so the user can correct the configuration.
#
# CSV saving:
# - The CSV writer writes a header then all collected rows; the out directory is
#   created if necessary. Errors are reported to the user.
#
# This file is intended to be run as a standalone GUI application.
#
# Author: Juan M. Gandarias
# web: www.jmgandarias.com
# email: jmgandarias@uma.es

import sys
import time
import threading
import queue
import json
import tkinter as tk
from tkinter import ttk, messagebox, filedialog
import serial
import serial.tools.list_ports
import csv
from datetime import datetime
import os
from matplotlib.figure import Figure
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg

# Default serial settings and reader sleep interval
DEFAULT_BAUD = 500000  # Must match Serial.begin() on ESP32
READ_THREAD_SLEEP_S = 0.01  # Sleep between polls in the reader thread
PLOT_UPDATE_MS = 100  # Plot refresh interval
MAX_PLOT_POINTS = 5000  # Limit points for performance
MAX_LOG_LINES_PER_TICK = 200  # Limit log processing per UI tick


class ControlGUI(tk.Tk):
    """Main GUI application for collecting experiment data from an ESP32.

    Responsibilities:
    - Present a simple UI to select a serial port, set an output CSV file,
      and start/stop experiments.
    - Manage the serial connection lifecycle.
    - Run a background reader thread that turns raw serial bytes into text
      lines and forwards them to the main thread for logging and parsing.
    - Collect parsed measurement rows in-memory (thread-safe) and save them to CSV.
    """

    def __init__(self):
        super().__init__()
        self.title("DC Motor GUI")
        self.geometry("720x576")

        # Set custom icon if available (try .ico first for Windows, then .png)
        try:
            icon_ico = os.path.join("images", "icon.ico")
            if os.path.exists(icon_ico):
                self.iconbitmap(icon_ico)
            else:
                icon_png = os.path.join("images", "icon.png")
                if os.path.exists(icon_png):
                    icon = tk.PhotoImage(file=icon_png)
                    self.iconphoto(True, icon)
        except Exception:
            pass  # If icon fails to load, use default

        # Serial and thread state
        # self.ser: an instance of serial.Serial when the port is open, else None
        self.ser = None
        # Background thread that continuously reads from serial
        self.read_thread = None
        # Event to request the reader thread stop; set from the main thread
        self.stop_event = threading.Event()
        # True while we are actively collecting experiment data (between Start and END)
        self.collecting = False
        # Queue used to deliver log/text messages from the reader thread to the UI thread
        self.lines_queue = queue.Queue()

        # In-memory buffer of parsed data rows. Each row is [power(float), pos(float), vel(float), time_ms(float)].
        # Access to this list must be guarded by data_lock.
        self.data_rows = []
        self.data_lock = threading.Lock()
        self.last_plot_len = 0

        # Initialize config file with default values
        self._initialize_config()

        # Build UI and start periodic queue processing on the main loop
        self._build_ui()
        self.after(100, self._process_lines_queue)

        # Ensure closing the window also closes the serial port
        self.protocol("WM_DELETE_WINDOW", self._on_exit)

        # Tracks whether the device has announced "READY" (used to avoid sending a start
        # byte too early after connecting / resetting the ESP32).
        self.ready_seen = False

    # -------------------- Initialization --------------------
    def _initialize_config(self):
        """Initialize config.json file with default values at startup."""
        config_path = os.path.join("config", "config.json")
        default_config = {
            "control_mode": "open-loop",
            "input_signal": "step",
            "ref": 0.0,
            "Kp": 1.0,
            "Ki": 0.0,
            "Kd": 0.0,
            "experiment_duration": 10.0,
            "sampling_rate": 0.01,
            "dead_zone_compensation": True
        }
        
        try:
            os.makedirs("config", exist_ok=True)
            with open(config_path, 'w') as f:
                json.dump(default_config, f, indent=4)
        except Exception as e:
            print(f"Warning: Could not initialize config file: {e}")

    # -------------------- UI --------------------
    def _build_ui(self):
        """Create and lay out all UI widgets."""
        pad = {"padx": 8, "pady": 6}

        # Connection frame
        conn = ttk.LabelFrame(self, text="Connection")
        conn.pack(fill="x", **pad)

        ttk.Label(conn, text="Port:").grid(row=0, column=0, sticky="w", **pad)
        self.port_cmb = ttk.Combobox(
            conn, width=36, state="readonly", values=self._list_ports()
        )
        self.port_cmb.grid(row=0, column=1, sticky="w", **pad)

        self.refresh_btn = ttk.Button(conn, text="Refresh", command=self._refresh_ports)
        self.refresh_btn.grid(row=0, column=2, sticky="w", **pad)

        ttk.Label(conn, text="Baud:").grid(row=1, column=0, sticky="w", **pad)
        self.baud_cmb = ttk.Combobox(
            conn,
            width=12,
            state="readonly",
            values=[
                9600,
                19200,
                38400,
                57600,
                115200,
                230400,
                250000,
                460800,
                500000,
                921600,
                1000000,
                2000000,
            ],
        )
        # If you run your ESP32 at 2,000,000 baud by default, do:
        # self.baud_cmb.set("2000000")
        self.baud_cmb.set(str(DEFAULT_BAUD))
        self.baud_cmb.grid(row=1, column=1, sticky="w", **pad)

        # New: Connect / Disconnect buttons
        self.connect_btn = ttk.Button(conn, text="Connect", command=self._on_connect)
        self.connect_btn.grid(row=0, column=3, sticky="w", **pad)
        self.disconnect_btn = ttk.Button(
            conn, text="Disconnect", command=self._on_disconnect, state="disabled"
        )
        self.disconnect_btn.grid(row=0, column=4, sticky="w", **pad)

        # --- Output frame (add this) ---
        out = ttk.LabelFrame(self, text="Output")
        out.pack(fill="x", **pad)

        ttk.Label(out, text="CSV file:").grid(row=0, column=0, sticky="w", **pad)

        # The actual entry the rest of the code expects:
        self.file_entry = ttk.Entry(out, width=50)
        self.file_entry.grid(row=0, column=1, sticky="we", **pad)
        out.columnconfigure(1, weight=1)

        # Sensible default path: ~/Documents if it exists, else CWD, with timestamped name.
        default_name = datetime.now().strftime("esp32_step_%Y%m%d_%H%M%S.csv")
        # Use ./experiment_data as default output directory (create if missing)
        default_dir = os.path.join(os.getcwd(), "experiment_data")
        try:
            os.makedirs(default_dir, exist_ok=True)
        except Exception:
            # Fallback to current working directory if creation fails
            default_dir = os.getcwd()
        default_path = os.path.normpath(os.path.join(default_dir, default_name))
        self.file_entry.insert(0, default_path)

        # Browse button already wired to _browse_outfile()
        browse_btn = ttk.Button(out, text="Browse…", command=self._browse_outfile)
        browse_btn.grid(row=0, column=2, **pad)

        # --- Top area: Configuration (left) and Controls (right) inside a resizable PanedWindow ---
        # Create a vertical PanedWindow so the user can drag the sash to resize
        self.paned = tk.PanedWindow(self, orient="vertical")
        self.paned.pack(fill="both", expand=True, **pad)

        # The left/top pane: container for Configuration and (visual) Controls frame
        top = ttk.Frame(self.paned)
        self.paned.add(top)

        # Configuration frame on the left (expands to take available width)
        config_frame = ttk.LabelFrame(top, text="Configuration")
        config_frame.pack(side="left", fill="x", expand=True, **pad)

        ttk.Label(config_frame, text="Control Mode:").grid(row=0, column=0, sticky="w", **pad)
        self.control_mode_cmb = ttk.Combobox(
            config_frame,
            width=20,
            state="readonly",
            values=["open-loop", "position", "velocity"]
        )
        self.control_mode_cmb.set("open-loop")
        self.control_mode_cmb.grid(row=0, column=1, sticky="w", **pad)

        # PID gains on same row as Control Mode
        # Make Kp label align to the right of its column and reduce horizontal gap
        ttk.Label(config_frame, text="Kp:").grid(row=0, column=2, sticky="e", padx=2, pady=6)
        self.kp_entry = ttk.Entry(config_frame, width=10)
        self.kp_entry.insert(0, "1.0")
        # Keep vertical padding consistent, use a small left padx so entry sits close to the label
        self.kp_entry.grid(row=0, column=3, sticky="w", padx=6, pady=6)

        ttk.Label(config_frame, text="Ki:").grid(row=0, column=4, sticky="w", **pad)
        self.ki_entry = ttk.Entry(config_frame, width=10)
        self.ki_entry.insert(0, "0.0")
        self.ki_entry.grid(row=0, column=5, sticky="w", **pad)

        ttk.Label(config_frame, text="Kd:").grid(row=0, column=6, sticky="w", **pad)
        self.kd_entry = ttk.Entry(config_frame, width=10)
        self.kd_entry.insert(0, "0.0")
        self.kd_entry.grid(row=0, column=7, sticky="w", **pad)

        ttk.Label(config_frame, text="Input Signal:").grid(row=1, column=0, sticky="w", **pad)
        self.input_signal_cmb = ttk.Combobox(
            config_frame,
            width=20,
            state="readonly",
            values=["step", "ramp", "manual"]
        )
        self.input_signal_cmb.set("step")
        self.input_signal_cmb.grid(row=1, column=1, sticky="w", **pad)

        # Manual Ref control: label "Ref:" and editable entry to its right.
        # Hidden unless input_signal == "manual". Range [-100, 100], default 0.00.
        self.manual_ref_strvar = tk.StringVar(value="0.00")
        ttk.Label(config_frame, text="Ref:").grid(row=1, column=2, sticky="e", padx=(6, 2))
        self.manual_ref_entry = ttk.Entry(config_frame, width=8, textvariable=self.manual_ref_strvar, justify="right")
        # Place entry to the right of label (column 3)
        self.manual_ref_entry.grid(row=1, column=3, sticky="w", padx=6)
        # Add "Set Ref" button to send the value over serial when pressed
        self.set_ref_btn = ttk.Button(config_frame, text="Set Ref", command=self._on_set_ref)
        self.set_ref_btn.grid(row=1, column=4, sticky="w", padx=6)
        # Initially hidden
        self.manual_ref_entry.grid_remove()
        self.set_ref_btn.grid_remove()
        # Bind visibility and entry events
        self.input_signal_cmb.bind("<<ComboboxSelected>>", self._on_input_signal_change)
        # Validate/format on focus out; do NOT send to serial here.
        self.manual_ref_entry.bind("<FocusOut>", lambda e: self._on_manual_ref_entry_change())

        # Experiment duration and sampling rate on row 2
        ttk.Label(config_frame, text="Exp Duration (s):").grid(row=2, column=0, sticky="w", **pad)
        self.exp_duration_entry = ttk.Entry(config_frame, width=10)
        self.exp_duration_entry.insert(0, "10.0")
        self.exp_duration_entry.grid(row=2, column=1, sticky="w", **pad)

        ttk.Label(config_frame, text="Sampling Rate (s):").grid(row=2, column=2, sticky="w", **pad)
        self.sampling_rate_entry = ttk.Entry(config_frame, width=10)
        self.sampling_rate_entry.insert(0, "0.01")
        self.sampling_rate_entry.grid(row=2, column=3, sticky="w", **pad)

        self.dead_zone_comp_var = tk.BooleanVar(value=True)
        ttk.Checkbutton(
            config_frame,
            text="Dead-zone compensation",
            variable=self.dead_zone_comp_var,
            onvalue=True,
            offvalue=False,
        ).grid(row=0, column=8, sticky="w", **pad)

        # Button to send config (updates file and sends to ESP32)
        self.send_config_btn = ttk.Button(
            config_frame, text="Send Config", command=self._on_send_config, state="disabled"
        )
        self.send_config_btn.grid(row=2, column=5, columnspan=2, sticky="w", **pad)

        # Controls moved into the Configuration block (row 3)
        # Start is disabled until connected
        self.start_btn = ttk.Button(
            config_frame, text="Start Experiment", command=self._on_start, state="disabled"
        )
        self.start_btn.grid(row=3, column=0, **pad)

        self.stop_btn = ttk.Button(
            config_frame, text="Save", command=self._on_save, state="disabled"
        )
        self.stop_btn.grid(row=3, column=1, **pad)

        self.stop_no_save_btn = ttk.Button(
            config_frame, text="Stop", command=self._on_stop_without_save, state="disabled"
        )
        self.stop_no_save_btn.grid(row=3, column=2, **pad)

        # Visible sash handle: place it INSIDE the top pane (bottom of top) so the PanedWindow keeps two panes.
        sash_handle = tk.Frame(top, height=12, bg=self.cget("bg"))
        arrow = tk.Label(sash_handle, text="⇅", fg="black", bg=self.cget("bg"))
        arrow.pack(expand=True)
        sash_handle.configure(cursor="sb_v_double_arrow")
        # Bind dragging so clicking the arrow lets the user move the PanedWindow sash
        sash_handle.bind("<B1-Motion>", self._drag_paned)
        sash_handle.pack(side="bottom", fill="x", padx=0, pady=0)

        # Notebook with Plot and Log tabs goes into the lower pane of the PanedWindow
        self.notebook = ttk.Notebook(self.paned)
        self.paned.add(self.notebook)

        # Status text (keep outside the paned so it remains visible)
        self.status_var = tk.StringVar(value="Idle")
        ttk.Label(self, textvariable=self.status_var).pack(anchor="w", **pad)

        # Plot tab
        self.plot_tab = ttk.Frame(self.notebook)
        self.notebook.add(self.plot_tab, text="Live Plot")

        # Log tab
        self.log_tab = ttk.Frame(self.notebook)
        self.notebook.add(self.log_tab, text="Log")

        # Read-only log widget updated from the main thread by draining lines_queue
        self.log = tk.Text(self.log_tab, height=9, wrap="none", state="disabled")
        self.log.pack(fill="both", expand=True)

        # Initialize matplotlib figure in plot tab and schedule periodic updates
        self._init_plot()
        self.after(PLOT_UPDATE_MS, self._update_plot)

    def _drag_paned(self, event):
        """Handle dragging of the visible sash handle to resize the PanedWindow."""
        try:
            # Compute target y in paned coordinates
            paned_rooty = self.paned.winfo_rooty()
            y = event.y_root - paned_rooty
            # Constrain to sensible min/max so panes don't collapse
            min_top = 60
            min_bottom = 80
            paned_h = max(self.paned.winfo_height(), 200)
            max_y = paned_h - min_bottom
            y = max(min_top, min(y, max_y))
            # Place sash (between top and notebook) — sash index 0
            try:
                self.paned.sash_place(0, 0, int(y))
            except Exception:
                pass
        except Exception:
            pass

    # -------------------- Serial helpers --------------------
    def _list_ports(self):
        """Return a list of human-readable port strings for the combobox."""
        ports = serial.tools.list_ports.comports()
        return [f"{p.device} - {p.description}" for p in ports]

    def _refresh_ports(self):
        """Re-query available serial ports and update the combobox."""
        ports = self._list_ports()
        self.port_cmb["values"] = ports
        if len(ports) == 1:
            self.port_cmb.set(ports[0])
        self._log("Ports refreshed.")

    def _open_serial(self):
        """Open the selected serial port with the chosen baud rate.

        Returns True on success, False otherwise. Shows message boxes for user
        recoverable errors so the user may reconfigure.
        """
        if self.ser and self.ser.is_open:
            return True
        sel = self.port_cmb.get()
        if not sel:
            messagebox.showwarning("Select port", "Please select a serial port first.")
            return False
        port = sel.split(" - ")[0]
        try:
            self.ser = serial.Serial(
                port=port, baudrate=int(self.baud_cmb.get()), timeout=0.05
            )

            # Optional: pulse DTR to reset many ESP32 boards so they start in a known state.
            # This is best-effort; failure to toggle DTR is non-fatal.
            try:
                self.ser.dtr = False
                time.sleep(0.05)
                self.ser.dtr = True
            except Exception:
                pass

            # Clear any old buffered data on the port
            self.ser.reset_input_buffer()
            self.ser.reset_output_buffer()

            time.sleep(0.2)  # small settle time to let the device boot/respond
            self._log(f"Opened {port} @ {self.ser.baudrate} baud.")
            return True
        except Exception as e:
            messagebox.showerror("Serial error", f"Could not open port:\n{e}")
            return False

    def _close_serial(self):
        """Close serial port if open (safe to call multiple times)."""
        if self.ser:
            try:
                self.ser.close()
                self._log("Serial port closed.")
            except Exception:
                pass
            self.ser = None

    # -------------------- Actions --------------------
    def _on_start(self):
        """Handler for the Start button: send the single-byte '1' trigger to the ESP32.

        If the device has not yet announced READY, wait a short time before sending
        to avoid racing with the ESP32 boot sequence.
        """
        if not (self.ser and self.ser.is_open):
            messagebox.showwarning("Not connected", "Connect first.")
            return
        if not self.ready_seen:
            # Defer slightly to allow the device to finish booting/reset sequence
            self.status_var.set("Waiting for READY…")
            self._log("Device not READY yet; delaying send 1 by 800 ms.")
            self.after(800, self._send_start_trigger)  # send a bit later
        else:
            self._send_start_trigger()

    def _send_start_trigger(self):
        """Clear prior data, flip UI into collecting state, and send 'START' command."""
        # Clear in-memory data buffer
        with self.data_lock:
            self.data_rows = []
        # Reset plotting state so a new run starts from empty axes
        self.last_plot_len = 0
        try:
            # Clear plotted lines and autoscale so old data doesn't interfere
            self.line_power.set_data([], [])
            self.line_pos.set_data([], [])
            self.line_pos_ref.set_data([], [])
            self.line_vel.set_data([], [])
            self.line_vel_filt.set_data([], [])
            self.line_vel_ref.set_data([], [])
            # Recompute limits for all axes
            try:
                self.ax_power.relim()
                self.ax_power.autoscale_view()
            except Exception:
                pass
            self.ax_pos.relim()
            self.ax_pos.autoscale_view()
            self.ax_vel.relim()
            self.ax_vel.autoscale_view()
            self.canvas.draw_idle()
        except Exception:
            # If plot not yet initialized or other error, ignore and continue
            pass

        self.collecting = True
        self.start_btn["state"] = "disabled"
        self.stop_btn["state"] = "normal"
        self.stop_no_save_btn["state"] = "normal"
        self.send_config_btn["state"] = "disabled"
        self.status_var.set("Experiment running…")
        try:
            # Clear buffers to avoid mixing stale lines with the new run
            self.ser.reset_input_buffer()
            self.ser.reset_output_buffer()
            # Protocol: send 'START' command to instruct the ESP32 to start recording
            self.ser.write(b"START\n")
            self.ser.flush()
            self._log("Sent start trigger: 'START'")
        except Exception as e:
            messagebox.showerror("Serial error", f"Failed to write start command:\n{e}")
            # Attempt a graceful stop without saving when the write fails
            self._on_stop_and_save(force_save=False)

    def _on_stop_and_save(self, force_save=True):
        """Stop collection and, unless force_save==False, save the collected rows to CSV."""
        # Send END command to ESP32 to stop the experiment
        if self.ser and self.ser.is_open:
            try:
                self.ser.write(b"END\n")
                self.ser.flush()
                self._log("Sent stop command: 'END'")
            except Exception as e:
                messagebox.showerror("Serial error", f"Failed to write stop command:\n{e}")
        
        self.collecting = False
        self.stop_btn["state"] = "normal"
        self.stop_no_save_btn["state"] = "disabled"
        self.start_btn["state"] = "normal"
        # Re-enable Send Config button when experiment stops
        if self.ser and self.ser.is_open:
            self.send_config_btn["state"] = "normal"
        saved = False
        if force_save:
            saved = self._save_csv()
        self.status_var.set("Saved and ready." if saved else "Ready.")

    def _on_save(self):
        """Save current data without stopping the experiment."""
        saved = self._save_csv()
        if saved:
            self.status_var.set("Saved.")

    def _on_stop_without_save(self):
        """Stop collection without saving; keep data in memory so user can save afterward."""
        self._on_stop_and_save(force_save=False)

    def _on_send_config(self):
        """Update config.json file and send the configuration to the ESP32 via serial."""
        if not (self.ser and self.ser.is_open):
            messagebox.showwarning("Not connected", "Connect first.")
            return
        
        config_path = os.path.join("config", "config.json")
        
        try:
            # Get values from comboboxes and entries
            control_mode = self.control_mode_cmb.get()
            input_signal = self.input_signal_cmb.get()
            # Manual ref (always include in config); rounded to 2 decimals
            try:
                ref_val = round(float(self.manual_ref_strvar.get()), 2)
            except Exception:
                ref_val = 0.0
            # Parse PID gains as floats
            try:
                kp = float(self.kp_entry.get())
                ki = float(self.ki_entry.get())
                kd = float(self.kd_entry.get())
                exp_duration = float(self.exp_duration_entry.get())
                sampling_rate = float(self.sampling_rate_entry.get())
            except ValueError:
                messagebox.showerror("Invalid Input", "Kp, Ki, Kd, Exp Duration, and Sampling Rate must be valid numbers.")
                return
            
            # Validate maximum 5 decimals
            def validate_decimals(value_str):
                """Check if a number string has more than 5 decimals."""
                if '.' in value_str:
                    decimals = len(value_str.split('.')[1])
                    return decimals <= 5
                return True
            
            kp_str = self.kp_entry.get().strip()
            ki_str = self.ki_entry.get().strip()
            kd_str = self.kd_entry.get().strip()
            
            if not validate_decimals(kp_str):
                messagebox.showerror("Invalid Format", "Kp can have maximum 5 decimal places.")
                return
            if not validate_decimals(ki_str):
                messagebox.showerror("Invalid Format", "Ki can have maximum 5 decimal places.")
                return
            if not validate_decimals(kd_str):
                messagebox.showerror("Invalid Format", "Kd can have maximum 5 decimal places.")
                return
            
            # Validate PID gains are within acceptable ranges
            if kp < 0 or kp > 2000:
                messagebox.showerror("Invalid Range", "Kp must be between 0 and 30.")
                return
            if ki < 0 or ki > 1000:
                messagebox.showerror("Invalid Range", "Ki must be between 0 and 10.")
                return
            if kd < 0 or kd > 1000:
                messagebox.showerror("Invalid Range", "Kd must be between 0 and 10.")
                return
            
            # Validate experiment duration and sampling rate ranges
            if exp_duration < 0 or exp_duration > 3e38:
                messagebox.showerror("Invalid Range", "Experiment Duration must be between 0 and 3e38 seconds.")
                return
            if sampling_rate < 0.001 or sampling_rate > 1000:
                messagebox.showerror("Invalid Range", "Sampling Rate must be between 0.001 and 1000 seconds.")
                return
            
            # Create JSON object
            config_obj = {
                "control_mode": control_mode,
                "input_signal": input_signal,
                "ref": ref_val,
                "Kp": kp,
                "Ki": ki,
                "Kd": kd,
                "experiment_duration": exp_duration,
                "sampling_rate": sampling_rate,
                "dead_zone_compensation": bool(self.dead_zone_comp_var.get())
            }
            
            # First, update the config.json file
            os.makedirs("config", exist_ok=True)
            with open(config_path, 'w') as f:
                json.dump(config_obj, f, indent=4)
            self._log(f"Config file updated: {config_path}")
            
            # Then, send to ESP32 as compact JSON (single line, no whitespace)
            config_data = json.dumps(config_obj, separators=(',', ':'))
            self.ser.write(config_data.encode('utf-8'))
            self.ser.write(b'\n')  # Add newline to signal end of JSON
            self.ser.flush()
            
            self._log(f"Config sent to ESP32: {config_data}")
            self.status_var.set("Config updated and sent.")
            messagebox.showinfo("Success", "Configuration file updated and sent to ESP32!")
        except Exception as e:
            messagebox.showerror("Send Error", f"Failed to send config:\n{e}")
            self._log(f"Error sending config: {e}")

    def _on_exit(self):
        """Gracefully exit the application: stop thread, close serial and destroy window."""
        # Signal reader thread to stop and wait briefly for it
        self.stop_event.set()
        self.collecting = False
        try:
            if self.read_thread and self.read_thread.is_alive():
                self.read_thread.join(timeout=1.0)
        except Exception:
            pass
        self._close_serial()
        self.destroy()

    def _on_connect(self):
        """Open the selected serial port and (if not running) start the reader thread."""
        if not self._open_serial():
            return

        # Start the reader thread if not already running
        if self.read_thread is None or not self.read_thread.is_alive():
            self.stop_event.clear()
            # Daemon thread so it won't prevent process exit
            self.read_thread = threading.Thread(target=self._reader_loop, daemon=True)
            self.read_thread.start()

        # Clear any stale data
        try:
            self.ser.reset_input_buffer()
            self.ser.reset_output_buffer()
        except Exception:
            pass

        self._log(f"Connecting to {self.ser.port} @ {self.ser.baudrate} baud.")
        self.status_var.set("Connected. Ready to start.")
        # Update UI states
        self.connect_btn["state"] = "disabled"
        self.disconnect_btn["state"] = "normal"
        self.start_btn["state"] = "normal"
        self.stop_btn["state"] = "disabled"
        self.stop_no_save_btn["state"] = "disabled"
        self.send_config_btn["state"] = "normal"

    def _on_disconnect(self):
        """Stop the reader thread, close the port, and update the UI."""
        self.collecting = False
        self.stop_event.set()
        try:
            if self.read_thread and self.read_thread.is_alive():
                self.read_thread.join(timeout=1.0)
        except Exception:
            pass
        self.read_thread = None

        self._close_serial()

        self.status_var.set("Disconnected.")
        self._log("Disconnected.")
        # Update UI states
        self.connect_btn["state"] = "normal"
        self.disconnect_btn["state"] = "disabled"
        self.start_btn["state"] = "disabled"
        self.stop_btn["state"] = "disabled"
        self.stop_no_save_btn["state"] = "disabled"
        self.send_config_btn["state"] = "disabled"

    def _reconnect_and_resend_config(self):
        """Disconnect, wait 1s, reconnect, wait 1s, then send current config."""
        # Step 1: Disconnect
        self._on_disconnect()
        self._log("Disconnecting to reset device...")
        
        # Step 2: Wait 1 second before reconnecting
        self.after(1000, self._reconnect_step)

    def _reconnect_step(self):
        """Step 2: Reconnect to the device."""
        self._log("Reconnecting to device...")
        self._on_connect()
        
        # Step 3: Wait 1 second before sending config
        self.after(1000, self._send_config_step)

    def _send_config_step(self):
        """Step 3: Send the current configuration."""
        self._log("Sending configuration...")
        if self.ser and self.ser.is_open:
            self._on_send_config()

    # -------------------- Reader & parsing --------------------
    def _reader_loop(self):
        """Background thread loop that reads raw bytes from serial and yields complete lines.

        Implementation details:
        - Maintains a small local byte buffer to accumulate partial reads.
        - Extracts lines split by LF (\\n). Carriage returns are stripped when decoding.
        - Posts every decoded, non-empty line to the lines_queue for display.
        - Calls _handle_line() on each line (runs in the reader thread); that method
          is careful to only perform thread-safe actions and to schedule UI work on
          the main thread via self.after(...) when necessary.
        - Sleeps briefly if no data is available (controlled by READ_THREAD_SLEEP_S)
          to avoid pegging the CPU.
        """
        buffer = b""
        while not self.stop_event.is_set():
            try:
                if self.ser and self.ser.is_open:
                    # Read up to 1024 bytes using the serial timeout to avoid blocking forever
                    chunk = self.ser.read(1024)
                    if chunk:
                        buffer += chunk
                        # Process all complete lines available in buffer
                        while b"\n" in buffer:
                            line, buffer = buffer.split(b"\n", 1)
                            # Decode line robustly and strip whitespace
                            line = line.strip().decode(errors="ignore")
                            if line:
                                self._handle_line(line)
                    else:
                        # No data this iteration: sleep briefly to avoid busy-looping
                        time.sleep(READ_THREAD_SLEEP_S)
                else:
                    # Serial not open: sleep a bit and retry
                    time.sleep(0.2)
            except Exception as e:
                # Forward read errors to UI log and continue; don't let the thread die silently.
                self.lines_queue.put(("log", f"[Serial read error] {e}"))
                time.sleep(0.2)

    def _handle_line(self, line: str):
        """Process a single text line from the ESP32.

                Supported data line formats:
                - Legacy CSV-like row: "power;pos;vel;vel_filtered;ref;time_ms"
                - JSON batch row with vectors:
                    {"type":"DATA_BATCH","power":[...],"pos":[...],"vel":[...],
                     "vel_filtered":[...],"ref":[...],"time_ms":[...]} 

        Special control lines:
          - "READY": device readiness handshake; recorded in self.ready_seen and
                     optionally updates the status label.
          - "END": signals end of the experiment; schedules a save on the main thread.

        This function:
        - Always posts the raw line to the UI log via lines_queue.
                - If collecting==True and the line matches either supported data format,
                    parses rows and appends them to data_rows under data_lock.
        - Ignores malformed lines silently.
        """
        # Echo every line in the log (UI thread will display)
        self.lines_queue.put(("log", line))

        if line.strip() == "READY":
            self.ready_seen = True
            # Update status on the main thread immediately
            self.after(0, lambda: self.status_var.set("Connected. READY from device."))
            return

        # If ESP32 signals end of experiment with "END", stop & save on main thread
        if line == "END":
            # Use after() to ensure UI actions run in main thread.
            # Stop collection but DO NOT save automatically; let the user press "Save".
            def on_end():
                self._on_stop_and_save(force_save=False)
                # Re-enable Send Config button after experiment ends
                if self.ser and self.ser.is_open:
                    self.send_config_btn["state"] = "normal"
            self.after(0, on_end)
            return

        # If not currently collecting, ignore parsed data
        if not self.collecting:
            return

        # Parse legacy semicolon-separated fields first (backward compatibility).
        parts = line.split(";")
        if len(parts) == 6:
            try:
                # Accept integer or float-looking values, be tolerant to formatting
                power = float(parts[0])
                pos = float(parts[1])
                vel = float(parts[2])
                velf = float(parts[3])
                ref = float(parts[4])
                tms = float(parts[5])  # milliseconds reported by ESP32
            except ValueError:
                return

            with self.data_lock:
                self.data_rows.append([power, pos, vel, velf, ref, tms])
            return

        # Parse new JSON batched payload.
        if not line.startswith("{"):
            return

        try:
            payload = json.loads(line)
        except Exception:
            return

        if not isinstance(payload, dict) or payload.get("type") != "DATA_BATCH":
            return

        power_vec = payload.get("power")
        pos_vec = payload.get("pos")
        vel_vec = payload.get("vel")
        velf_vec = payload.get("vel_filtered")
        ref_vec = payload.get("ref")
        time_vec = payload.get("time_ms")

        vectors = [power_vec, pos_vec, vel_vec, velf_vec, ref_vec, time_vec]
        if not all(isinstance(v, list) for v in vectors):
            return

        n = min(len(power_vec), len(pos_vec), len(vel_vec), len(velf_vec), len(ref_vec), len(time_vec))
        if n <= 0:
            return

        parsed_rows = []
        for i in range(n):
            try:
                parsed_rows.append([
                    float(power_vec[i]),
                    float(pos_vec[i]),
                    float(vel_vec[i]),
                    float(velf_vec[i]),
                    float(ref_vec[i]),
                    float(time_vec[i]),
                ])
            except (TypeError, ValueError):
                # Skip malformed samples while keeping the order of valid samples.
                continue

        if not parsed_rows:
            return

        with self.data_lock:
            self.data_rows.extend(parsed_rows)

        dropped = payload.get("dropped")
        if isinstance(dropped, (int, float)) and dropped > 0:
            self.lines_queue.put(("log", f"[WARN] Device dropped samples: {int(dropped)}"))

    # -------------------- Save CSV --------------------
    def _save_csv(self):
        """Write collected rows to CSV file. Returns True on success.

        Behavior:
        - Copies data_rows under lock to avoid holding the lock while performing IO.
        - If no rows are present, logs and returns False.
        - Ensures the output directory exists.
        - Writes a header then the saved rows using csv.writer.
        - Reports success or failure via the UI log and message boxes.
        """
        with self.data_lock:
            rows = list(self.data_rows)

        if not rows:
            self._log("No data to save.")
            return False

        out_path = self.file_entry.get().strip()
        if not out_path:
            messagebox.showwarning(
                "Missing path", "Please specify an output .csv filename."
            )
            return False
        if not out_path.lower().endswith(".csv"):
            out_path += ".csv"

        # Expand user home directory and normalize path
        out_path = os.path.normpath(os.path.expanduser(out_path))
        out_dir = os.path.dirname(out_path)
        
        # If dirname is empty, use current directory
        if not out_dir:
            out_dir = os.getcwd()
        
        # Validate the final path
        if not out_path or len(out_path) == 0:
            messagebox.showerror("Save error", "Invalid file path.")
            return False
        
        self._log(f"Save path: {out_path}")
        self._log(f"Save directory: {out_dir}")
        
        # Create directory if it doesn't exist
        try:
            os.makedirs(out_dir, exist_ok=True)
            self._log(f"Directory created/verified: {out_dir}")
        except OSError as e:
            error_msg = e.strerror or "Unable to create folder"
            self._log(f"Directory creation failed: {out_dir} - {error_msg}")
            messagebox.showerror(
                "Save error",
                f"Cannot create directory:\n{out_dir}\n\nError: {error_msg}",
            )
            return False

        try:
            self._log(f"Attempting to write file: {out_path}")
            # Write header + data rows
            with open(out_path, mode="w", newline="", encoding="utf-8") as f:
                writer = csv.writer(f)
                writer.writerow(["voltage", "pos_rad", "vel_rad_per_s", "vel_filtered_rad_per_s", "ref", "time_ms"])
                writer.writerows(rows)
            self._log(f"Saved {len(rows)} rows to: {out_path}")
            messagebox.showinfo("Success", f"Data saved successfully to:\n{out_path}")
            return True
        except Exception as e:
            error_msg = str(e)
            self._log(f"File write failed: {error_msg}")
            messagebox.showerror(
                "Save error",
                f"Cannot write to file:\n{out_path}\n\nError: {error_msg}",
            )
            return False

    # -------------------- UI helpers --------------------
    def _browse_outfile(self):
        """Open file-save dialog and update file_entry with chosen path."""
        initial = self.file_entry.get()
        if not initial.lower().endswith(".csv"):
            initial += ".csv"
        file = filedialog.asksaveasfilename(
            title="Choose output .csv file",
            defaultextension=".csv",
            filetypes=[("CSV files", "*.csv")],
            initialfile=os.path.basename(initial),
            initialdir=(
                os.path.dirname(initial) if os.path.dirname(initial) else os.getcwd()
            ),
        )
        if file:
            self.file_entry.delete(0, tk.END)
            self.file_entry.insert(0, file)

    def _process_lines_queue(self):
        """Periodically called on the main thread to drain the lines queue and append to the log widget.

        This keeps all Tkinter UI updates on the main thread while allowing the
        reader thread to run without interacting with Tk directly.
        """
        drained = 0
        try:
            while drained < MAX_LOG_LINES_PER_TICK:
                kind, payload = self.lines_queue.get_nowait()
                if kind == "log":
                    self._log(payload)
                drained += 1
        except queue.Empty:
            pass
        # Re-schedule
        self.after(10 if drained >= MAX_LOG_LINES_PER_TICK else 100, self._process_lines_queue)

    def _log(self, message: str):
        """Append a line to the read-only log text widget (must be called from main thread)."""
        self.log.configure(state="normal")
        self.log.insert("end", message + "\n")
        self.log.see("end")
        self.log.configure(state="disabled")

    # -------------------- Plot helpers --------------------
    def _init_plot(self):
        # Create matplotlib Figure with three subplots: power, position and velocity vs time
        # Controls toolbar (pause/resume, series selection)
        toolbar = ttk.Frame(self.plot_tab)
        toolbar.pack(fill="x", padx=8, pady=4)

        self.plot_paused = False
        self.pause_btn = ttk.Button(toolbar, text="Pause Plot", command=self._toggle_plot_pause)
        self.pause_btn.pack(side="left")

        # Series selection checkboxes (include Voltage)
        self.show_power_var = tk.IntVar(value=1)
        self.show_pos_var = tk.IntVar(value=1)
        self.show_vel_var = tk.IntVar(value=1)
        self.show_velf_var = tk.IntVar(value=1)

        ttk.Checkbutton(toolbar, text="Voltage", variable=self.show_power_var, command=self._on_series_toggle).pack(side="left", padx=6)
        ttk.Checkbutton(toolbar, text="Position", variable=self.show_pos_var, command=self._on_series_toggle).pack(side="left", padx=6)
        ttk.Checkbutton(toolbar, text="Velocity", variable=self.show_vel_var, command=self._on_series_toggle).pack(side="left", padx=6)
        ttk.Checkbutton(toolbar, text="Velocity (filtered)", variable=self.show_velf_var, command=self._on_series_toggle).pack(side="left", padx=6)

        # Use 3 rows: Voltage (top), Position (middle), Velocity (bottom). Share x-axis.
        self.fig = Figure(figsize=(6, 5), dpi=100)
        self.ax_power = self.fig.add_subplot(3, 1, 1)
        self.ax_pos = self.fig.add_subplot(3, 1, 2, sharex=self.ax_power)
        self.ax_vel = self.fig.add_subplot(3, 1, 3, sharex=self.ax_power)

        self.ax_power.set_ylabel("Voltage [V]")
        self.ax_pos.set_ylabel("pos [rad]")
        self.ax_vel.set_ylabel("vel [rad/s]")
        self.ax_vel.set_xlabel("time [s]")
        self.ax_power.grid(True, linestyle=":", alpha=0.6)
        self.ax_pos.grid(True, linestyle=":", alpha=0.6)
        self.ax_vel.grid(True, linestyle=":", alpha=0.6)

        (self.line_power,) = self.ax_power.plot([], [], color="tab:red", label="Voltage", linewidth=1.5, antialiased=True)
        (self.line_pos,) = self.ax_pos.plot([], [], color="tab:blue", label="pos", linewidth=1.5, antialiased=True)
        (self.line_pos_ref,) = self.ax_pos.plot([], [], color="tab:cyan", label="ref", linewidth=1.5, linestyle="--", antialiased=True)
        (self.line_vel,) = self.ax_vel.plot([], [], color="tab:orange", label="vel", linewidth=1.5, antialiased=True)
        (self.line_vel_filt,) = self.ax_vel.plot([], [], color="tab:green", label="vel_filt", linewidth=1.5, antialiased=True)
        (self.line_vel_ref,) = self.ax_vel.plot([], [], color="tab:cyan", label="ref", linewidth=1.5, linestyle="--", antialiased=True)
        
        # Add saturation lines for power (12V and -12V)
        self.ax_power.axhline(y=12, color="gray", linestyle="--", linewidth=1, alpha=0.7, label="saturation")
        self.ax_power.axhline(y=-12, color="gray", linestyle="--", linewidth=1, alpha=0.7)
        
        self.ax_power.legend(loc="upper right")
        self.ax_pos.legend(loc="upper right")
        self.ax_vel.legend(loc="upper right")

        self.canvas = FigureCanvasTkAgg(self.fig, master=self.plot_tab)
        self.canvas.draw()
        self.canvas.get_tk_widget().pack(fill="both", expand=True)

        # Apply initial visibility based on checkboxes
        self._on_series_toggle()

    def _update_plot(self):
        # If paused, skip updating the plot (but keep scheduling)
        if self.plot_paused:
            self.after(PLOT_UPDATE_MS, self._update_plot)
            return

        # Copy data under lock to avoid blocking reader
        with self.data_lock:
            rows = list(self.data_rows)

        if len(rows) == self.last_plot_len:
            self.after(PLOT_UPDATE_MS, self._update_plot)
            return
        self.last_plot_len = len(rows)

        if rows:
            # Each row is [power, pos, vel, vel_filtered, ref, tms]
            # Limit number of points for performance
            if len(rows) > MAX_PLOT_POINTS:
                rows = rows[-MAX_PLOT_POINTS:]
            t = [r[5] / 1000.0 for r in rows]
            power = [r[0] for r in rows]
            pos = [r[1] for r in rows]
            vel = [r[2] for r in rows]
            velf = [r[3] for r in rows]
            ref = [r[4] for r in rows]

            # Update series data
            self.line_power.set_data(t, power)
            self.line_pos.set_data(t, pos)
            self.line_vel.set_data(t, vel)
            self.line_vel_filt.set_data(t, velf)
            
            # Update ref lines based on control mode
            control_mode = self.control_mode_cmb.get()
            if control_mode == "position":
                self.line_pos_ref.set_data(t, ref)
                self.line_pos_ref.set_visible(True)
                self.line_vel_ref.set_visible(False)
            elif control_mode == "velocity":
                self.line_vel_ref.set_data(t, ref)
                self.line_vel_ref.set_visible(True)
                self.line_pos_ref.set_visible(False)
            else:  # open-loop
                self.line_pos_ref.set_visible(False)
                self.line_vel_ref.set_visible(False)

            # Autoscale axes to data
            try:
                self.ax_power.relim()
                self.ax_power.autoscale_view()
            except Exception:
                pass
            try:
                self.ax_pos.relim()
                self.ax_pos.autoscale_view()
            except Exception:
                pass
            try:
                self.ax_vel.relim()
                self.ax_vel.autoscale_view()
            except Exception:
                pass

            # Ensure x-limits cover data (set on the shared top axis)
            if t:
                try:
                    self.ax_power.set_xlim(t[0], t[-1] if t[-1] > t[0] else t[0] + 1e-3)
                except Exception:
                    pass

            self.canvas.draw_idle()

        # Schedule next update
        self.after(PLOT_UPDATE_MS, self._update_plot)

    def _toggle_plot_pause(self):
        self.plot_paused = not self.plot_paused
        try:
            self.pause_btn.configure(text="Resume Plot" if self.plot_paused else "Pause Plot")
        except Exception:
            pass

    def _on_series_toggle(self):
        # Toggle visibility based on checkboxes
        try:
            self.line_power.set_visible(bool(self.show_power_var.get()))
            self.line_pos.set_visible(bool(self.show_pos_var.get()))
            self.line_vel.set_visible(bool(self.show_vel_var.get()))
            self.line_vel_filt.set_visible(bool(self.show_velf_var.get()))
            self.canvas.draw_idle()
        except Exception:
            pass

    def _on_input_signal_change(self, event=None):
        """Show manual ref entry when input_signal is 'manual', hide otherwise."""
        try:
            if self.input_signal_cmb.get() == "manual":
                self.manual_ref_entry.grid()
                self.set_ref_btn.grid()
                # ensure formatted display
                try:
                    v = float(self.manual_ref_strvar.get())
                    self.manual_ref_strvar.set(f"{v:.2f}")
                except Exception:
                    self.manual_ref_strvar.set("0.00")
            else:
                self.manual_ref_entry.grid_remove()
                self.set_ref_btn.grid_remove()
        except Exception:
            pass

    def _on_manual_ref_entry_change(self):
        """Validate typed value, clamp to [-100,100], format to 2 decimals."""
        try:
            txt = self.manual_ref_strvar.get().strip()
            if txt == "":
                self.manual_ref_strvar.set("0.00")
                return
            v = float(txt)
            if v < -100.0:
                v = -100.0
            elif v > 100.0:
                v = 100.0
            v = round(v, 2)
            self.manual_ref_strvar.set(f"{v:.2f}")
        except Exception:
            # restore a valid formatted value
            try:
                v = float(self.manual_ref_strvar.get())
                self.manual_ref_strvar.set(f"{v:.2f}")
            except Exception:
                self.manual_ref_strvar.set("0.00")

    def _on_set_ref(self):
        """Handler for Set Ref button: validate value and send '<value>\\n' over serial."""
        try:
            txt = self.manual_ref_strvar.get().strip()
            if txt == "":
                messagebox.showwarning("Missing value", "Please enter a reference value.")
                return
            v = float(txt)
            if v < -100.0:
                v = -100.0
            elif v > 100.0:
                v = 100.0
            v = round(v, 2)
            # update displayed/formatted value
            self.manual_ref_strvar.set(f"{v:.2f}")

            if not (self.ser and getattr(self.ser, "is_open", False)):
                messagebox.showwarning("Not connected", "Open a serial connection first.")
                return

            # Compose command (use 'R' prefix as discussed) and send
            cmd = f"{v}\n".encode("utf-8")
            try:
                self.ser.write(cmd)
                self.ser.flush()
                self._log(f"Sent manual ref: {v:.2f}")
            except Exception as e:
                messagebox.showerror("Serial error", f"Failed to send Ref:\n{e}")
                self._log(f"Error sending Ref: {e}")
        except ValueError:
            messagebox.showerror("Invalid Input", "Reference must be a valid number.")
        except Exception as e:
            messagebox.showerror("Error", f"Unexpected error:\n{e}")
            self._log(f"Unexpected error sending Ref: {e}")


if __name__ == "__main__":
    app = ControlGUI()
    app.mainloop()