/*
  G-Code Sender GUI (ESP32) (python)
  By: Cade Myers
  Date: 3/7/2026
*/

Gcode_GUI.py
"""
G-code Sender GUI (ESP32) - Starter Project
-------------------------------------------
What this app does:
- Lets you paste/type G-code (or load a file)
- Streams G-code line-by-line to an ESP32 over Serial (USB)
- Uses a simple handshake: ESP32 should reply "ok" after receiving a command
- Includes DRY RUN mode so you can test UI logic without hardware connected

How to debug quickly:
- Watch the Log panel (right side)
- If nothing happens on Run All:
  - Confirm a COM port exists (Refresh)
  - Confirm you connected (status shows Connected)
  - Confirm your ESP32 firmware prints "ok" after receiving each line
- If Stop feels stuck:
  - It usually means the sender thread is waiting for "ok" from device.
  - This version supports Dry Run and improved timeout handling.

Dependency:
- pyserial  (install with:  py -m pip install pyserial)
"""

import tkinter as tk
from tkinter import ttk, filedialog, messagebox
import threading
import time

import serial
import serial.tools.list_ports


class GCodeGUI:
    def __init__(self, root: tk.Tk):
        # =========================
        # SECTION 0: App State Vars
        # =========================
        # ser: holds serial connection object when connected
        # is_running: True while streaming lines
        # stop_requested: flag used to stop a run loop safely
        # dry_run: True means we simulate sending w/out hardware
        self.root = root
        self.root.title("G-code Sender (ESP32)")

        self.ser = None
        self.is_running = False
        self.stop_requested = False

        # =========================
        # SECTION 1: Top Connection Bar
        # =========================
        top = ttk.Frame(root, padding=10)
        top.pack(fill="x")

        ttk.Label(top, text="Port:").pack(side="left")
        self.port_var = tk.StringVar(value="")
        self.port_combo = ttk.Combobox(top, textvariable=self.port_var, width=25, state="readonly")
        self.port_combo.pack(side="left", padx=6)

        ttk.Button(top, text="Refresh", command=self.refresh_ports).pack(side="left", padx=4)

        ttk.Label(top, text="Baud:").pack(side="left", padx=(12, 0))
        self.baud_var = tk.StringVar(value="115200")
        self.baud_entry = ttk.Entry(top, textvariable=self.baud_var, width=10)
        self.baud_entry.pack(side="left", padx=6)

        self.conn_btn = ttk.Button(top, text="Connect", command=self.toggle_connection)
        self.conn_btn.pack(side="left", padx=6)

        # Dry run toggle so you can test without ESP32 connected
        self.dry_run_var = tk.BooleanVar(value=True)
        ttk.Checkbutton(top, text="Dry Run (no device)", variable=self.dry_run_var).pack(side="left", padx=(12, 0))

        self.status_var = tk.StringVar(value="Disconnected")
        ttk.Label(top, textvariable=self.status_var).pack(side="left", padx=(12, 0))

        # =========================
        # SECTION 2: Main Body (Left: G-code editor, Right: Log)
        # =========================
        mid = ttk.Frame(root, padding=(10, 0, 10, 10))
        mid.pack(fill="both", expand=True)

        # ---- Left side: G-code editor + controls ----
        left = ttk.Frame(mid)
        left.pack(side="left", fill="both", expand=True)

        ttk.Label(left, text="G-code Input:").pack(anchor="w")
        self.gcode_text = tk.Text(left, height=18, wrap="none")
        self.gcode_text.pack(fill="both", expand=True, pady=(4, 0))

        controls = ttk.Frame(left, padding=(0, 8, 0, 0))
        controls.pack(fill="x")

        ttk.Button(controls, text="Load File", command=self.load_file).pack(side="left")
        ttk.Button(controls, text="Send Selected Line", command=self.send_selected_line).pack(side="left", padx=6)

        self.run_btn = ttk.Button(controls, text="Run All", command=self.run_all)
        self.run_btn.pack(side="left", padx=6)

        self.stop_btn = ttk.Button(controls, text="Stop", command=self.request_stop, state="disabled")
        self.stop_btn.pack(side="left", padx=6)

        ttk.Label(controls, text="Line delay (ms):").pack(side="left", padx=(18, 0))
        self.delay_var = tk.StringVar(value="0")
        ttk.Entry(controls, textvariable=self.delay_var, width=8).pack(side="left", padx=6)

        # ---- Right side: Log console ----
        right = ttk.Frame(mid, padding=(10, 0, 0, 0))
        right.pack(side="right", fill="both")

        ttk.Label(right, text="Log:").pack(anchor="w")
        self.log_text = tk.Text(right, height=18, width=55, wrap="word", state="disabled")
        self.log_text.pack(fill="both", expand=True, pady=(4, 0))

        log_controls = ttk.Frame(right, padding=(0, 8, 0, 0))
        log_controls.pack(fill="x")
        ttk.Button(log_controls, text="Clear Log", command=self.clear_log).pack(side="left")

        # =========================
        # SECTION 3: Init / Defaults
        # =========================
        self.refresh_ports()

        # Starter G-code example
        self.gcode_text.insert(
            "1.0",
            "G21\n"
            "G90\n"
            "G0 X0 Y0\n"
            "G1 X50 Y0 F300\n"
            "G1 X50 Y50\n"
            "G1 X0 Y50\n"
            "G1 X0 Y0\n"
            "M2\n"
        )

    # =========================
    # SECTION 4: Logging helpers
    # =========================
    def log(self, msg: str):
        """Append a line to the log panel."""
        self.log_text.configure(state="normal")
        self.log_text.insert("end", msg + "\n")
        self.log_text.see("end")
        self.log_text.configure(state="disabled")

    def clear_log(self):
        self.log_text.configure(state="normal")
        self.log_text.delete("1.0", "end")
        self.log_text.configure(state="disabled")

    # =========================
    # SECTION 5: Serial port discovery
    # =========================
    def refresh_ports(self):
        """Refresh COM port list (Windows) / tty list (Mac/Linux)."""
        ports = [p.device for p in serial.tools.list_ports.comports()]
        self.port_combo["values"] = ports
        if ports and self.port_var.get() not in ports:
            self.port_var.set(ports[0])
        if not ports:
            self.port_var.set("")

    # =========================
    # SECTION 6: Connect / Disconnect
    # =========================
    def toggle_connection(self):
        if self.ser and self.ser.is_open:
            self.disconnect()
        else:
            self.connect()

    def connect(self):
        port = self.port_var.get().strip()
        if not port:
            messagebox.showerror("No Port", "No serial port selected. Click Refresh and pick your ESP32 port.")
            return

        try:
            baud = int(self.baud_var.get().strip())
        except ValueError:
            messagebox.showerror("Bad Baud", "Baud must be an integer (e.g., 115200).")
            return

        try:
            self.ser = serial.Serial(port, baudrate=baud, timeout=0.2)  # short timeout helps Stop feel responsive
            time.sleep(1.2)  # device reset delay
            self.status_var.set(f"Connected: {port} @ {baud}")
            self.conn_btn.configure(text="Disconnect")
            self.log(f"[SYS] Connected to {port} @ {baud}")
        except Exception as e:
            messagebox.showerror("Connection Failed", str(e))

    def disconnect(self):
        try:
            if self.ser:
                self.ser.close()
        finally:
            self.ser = None
            self.status_var.set("Disconnected")
            self.conn_btn.configure(text="Connect")
            self.log("[SYS] Disconnected")

    # =========================
    # SECTION 7: Parsing / Sending
    # =========================
    def _is_comment_or_blank(self, line: str) -> bool:
        clean = line.strip()
        if not clean:
            return True
        # common comment styles: ;comment or (comment)
        if clean.startswith(";") or clean.startswith("("):
            return True
        return False

    def send_line_and_wait_ok(self, line: str) -> bool:
        """
        Sends one command line and waits for 'ok' (or error).
        Returns True if ok received (or line is blank/comment), False otherwise.

        STOP behavior:
        - If stop_requested becomes True, we bail out fast.
        - This prevents "stuck requesting stop".
        """
        if self._is_comment_or_blank(line):
            return True

        clean = line.strip()

        # Dry Run mode: simulate sending even when not connected
        if self.dry_run_var.get() and (not self.ser or not self.ser.is_open):
            self.log(f">>> {clean}  [DRY RUN]")
            # simulate a tiny processing time
            time.sleep(0.01)
            self.log("<<< ok  [DRY RUN]")
            return True

        # Not dry run and not connected -> error
        if not self.ser or not self.ser.is_open:
            self.log("[ERR] Not connected (and Dry Run is off).")
            return False

        try:
            self.ser.write((clean + "\n").encode("utf-8"))
            self.log(f">>> {clean}")

            # Wait up to max_wait seconds for ok/error,
            # but exit early if stop_requested
            max_wait = 2.5
            t0 = time.time()

            while time.time() - t0 < max_wait:
                if self.stop_requested:
                    self.log("[SYS] Stop flag detected while waiting for response.")
                    return False

                resp = self.ser.readline().decode("utf-8", errors="ignore").strip()
                if not resp:
                    continue

                self.log(f"<<< {resp}")
                low = resp.lower()
                if low == "ok":
                    return True
                if low.startswith("error"):
                    return False

            self.log("[ERR] Timeout waiting for ok.")
            return False
        except Exception as e:
            self.log(f"[ERR] Serial exception: {e}")
            return False

    # =========================
    # SECTION 8: File load
    # =========================
    def load_file(self):
        path = filedialog.askopenfilename(
            filetypes=[("G-code files", "*.gcode *.nc *.tap *.txt"), ("All files", "*.*")]
        )
        if not path:
            return
        try:
            with open(path, "r", encoding="utf-8", errors="ignore") as f:
                data = f.read()
            self.gcode_text.delete("1.0", "end")
            self.gcode_text.insert("1.0", data)
            self.log(f"[SYS] Loaded file: {path}")
        except Exception as e:
            messagebox.showerror("Load Failed", str(e))

    # =========================
    # SECTION 9: Run / Stop
    # =========================
    def get_delay_seconds(self) -> float:
        try:
            ms = int(self.delay_var.get().strip())
            return max(0, ms) / 1000.0
        except ValueError:
            return 0.0

    def request_stop(self):
        """
        Stop button handler:
        - Sets stop flag
        - Immediately re-enables UI if we are not actually running
          (prevents 'stuck requesting stop' when nothing is running)
        """
        self.stop_requested = True
        self.log("[SYS] Stop requested.")

        if not self.is_running:
            # If nothing is running, make the UI reflect that immediately.
            self.run_btn.configure(state="normal")
            self.stop_btn.configure(state="disabled")
            self.log("[SYS] Not currently running — stop cleared UI state.")

    def send_selected_line(self):
        # Pull the line under the cursor
        try:
            line = self.gcode_text.get("insert linestart", "insert lineend")
        except Exception:
            return

        if not line.strip():
            messagebox.showerror("No Command", "Selected line is empty. Type a command or select a non-empty line.")
            return

        def worker():
            ok = self.send_line_and_wait_ok(line)
            if not ok:
                self.log("[SYS] Selected line failed / stopped.")

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

    def _validate_gcode_not_empty(self) -> bool:
        """
        Requirement 3: error case for empty input.
        We treat 'empty' as: no non-comment, non-blank lines.
        """
        raw = self.gcode_text.get("1.0", "end")
        lines = raw.splitlines()
        meaningful = [ln for ln in lines if not self._is_comment_or_blank(ln)]
        if len(meaningful) == 0:
            messagebox.showerror(
                "No G-code",
                "Your G-code input is empty (or only comments). Paste/type at least one command before running."
            )
            return False
        return True

    def run_all(self):
        """
        Streams all G-code lines in a background thread.
        Uses stop_requested to stop safely.
        """
        if self.is_running:
            return

        if not self._validate_gcode_not_empty():
            return

        # If not connected, we can still run if Dry Run is enabled.
        if (not self.dry_run_var.get()) and (not self.ser or not self.ser.is_open):
            messagebox.showerror("Not Connected", "Connect to the ESP32, or enable Dry Run.")
            return

        self.is_running = True
        self.stop_requested = False
        self.run_btn.configure(state="disabled")
        self.stop_btn.configure(state="normal")
        self.log("[SYS] Starting run...")

        def worker():
            delay = self.get_delay_seconds()
            lines = self.gcode_text.get("1.0", "end").splitlines()

            for i, line in enumerate(lines, start=1):
                if self.stop_requested:
                    self.log("[SYS] Run stopped by user.")
                    break

                ok = self.send_line_and_wait_ok(line)
                if not ok:
                    # If stop was requested, that's expected; otherwise it's an error.
                    if self.stop_requested:
                        self.log("[SYS] Run stopped during send/wait.")
                    else:
                        self.log(f"[SYS] Error on line {i}. Stopping.")
                    break

                if delay > 0:
                    # Let Stop interrupt delay too
                    t0 = time.time()
                    while time.time() - t0 < delay:
                        if self.stop_requested:
                            break
                        time.sleep(0.01)
                    if self.stop_requested:
                        self.log("[SYS] Stopped during delay.")
                        break

            # Reset UI state
            self.is_running = False
            self.run_btn.configure(state="normal")
            self.stop_btn.configure(state="disabled")
            self.log("[SYS] Run complete.")

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


if __name__ == "__main__":
    root = tk.Tk()
    try:
        ttk.Style().theme_use("clam")
    except Exception:
        pass

    app = GCodeGUI(root)
    root.mainloop()

/*
  G-Code Interpreter (starter)
  By: Cade Myers
  Date: 3/7/2026
*/
/*
  ESP32 G-code Interpreter (Starter)
  ---------------------------------
  Reads newline-terminated G-code from Serial.
  Supports:
    G0, G1, G90, G91, G20, G21, M2, M6

  Execution model (simple):
    - For each G0/G1 line, compute target XY (abs/rel + units)
    - Run IK for that XY
    - Command servos to the resulting angles
    - Reply "ok"

  Notes:
    - For drawing straight lines, you'll later want to segment G1 into small steps.
    - This is the "get it working" baseline.
*/

#include <Arduino.h>
#include <ESP32Servo.h>
#include <math.h>

// -----------------------------
// USER SETTINGS (EDIT THESE)
// -----------------------------

// Servo pins (pick GPIO that supports PWM; avoid input-only pins)
static const int PIN_SHOULDER = 18;
static const int PIN_ELBOW    = 19;

// Link lengths in mm (MEASURE YOUR ARM)
static const float L1 = 190.0f;  // shoulder to elbow
static const float L2 = 190.0f;  // elbow to pen

// Servo calibration offsets (degrees)
// Use these to "zero" the arm to match your physical setup.
static const float SHOULDER_OFFSET_DEG = 90.0f;  // example: 90 means servo=90 is "straight out"
static const float ELBOW_OFFSET_DEG    = 90.0f;

// Servo safe limits (degrees)
static const float SHOULDER_MIN = 0.0f;
static const float SHOULDER_MAX = 180.0f;
static const float ELBOW_MIN    = 0.0f;
static const float ELBOW_MAX    = 180.0f;

// If you changed gearing to increase torque at the shoulder:
// For a torque-increasing ratio, shoulder moves LESS than servo.
// Example: servo pulley 20T, shoulder pulley 40T => ratio = 40/20 = 2
// shoulder_angle = servo_angle / ratio  (equivalently servo = shoulder * ratio)
static const float SHOULDER_TORQUE_RATIO = 2.0f; // <-- EDIT: driven/driver. If unknown, set 1.0

static const bool SIM_MODE = true;   // true = print angles only, false = actually move servos

// If SHOULDER_TORQUE_RATIO = driven/driver,
// then servo should command = joint_angle * SHOULDER_TORQUE_RATIO.
static inline float shoulderServoFromJoint(float shoulder_joint_deg) {
  return shoulder_joint_deg * SHOULDER_TORQUE_RATIO;
}

// -----------------------------
// G-code interpreter state
// -----------------------------
enum Units { MM, INCH };
enum Mode { ABSOLUTE, RELATIVE };

static Units units = MM;
static Mode mode = ABSOLUTE;

static float x_cur = 0.0f;
static float y_cur = 0.0f;

// last feed rate (not used yet for timing)
static float feed = 300.0f;

// Servos
Servo shoulderServo;
Servo elbowServo;

// Serial input buffer
static String lineBuf;

// -----------------------------
// Helpers
// -----------------------------
static inline float inchToMm(float v) { return v * 25.4f; }
static inline float rad2deg(float r) { return r * 180.0f / (float)M_PI; }
static inline float deg2rad(float d) { return d * (float)M_PI / 180.0f; }

static inline float clampf(float v, float lo, float hi) {
  return (v < lo) ? lo : (v > hi) ? hi : v;
}

static bool parseWordFloat(const String &s, char key, bool &found, float &value) {
  // Finds like X12.3 or Y-5 in a G-code line.
  int idx = s.indexOf(key);
  if (idx < 0) {
    found = false;
    return true;
  }

  int start = idx + 1;
  int end = start;
  while (end < (int)s.length()) {
    char c = s[end];
    if ((c >= '0' && c <= '9') || c == '.' || c == '-' || c == '+') {
      end++;
    } else {
      break;
    }
  }

  String num = s.substring(start, end);
  if (num.length() == 0) return false;

  value = num.toFloat();
  found = true;
  return true;
}

static void writeServos(float shoulder_joint_deg, float elbow_joint_deg) {
  float shoulder_cmd = shoulderServoFromJoint(shoulder_joint_deg) + SHOULDER_OFFSET_DEG;
  float elbow_cmd    = elbow_joint_deg + ELBOW_OFFSET_DEG;

  shoulder_cmd = clampf(shoulder_cmd, SHOULDER_MIN, SHOULDER_MAX);
  elbow_cmd    = clampf(elbow_cmd, ELBOW_MIN, ELBOW_MAX);

  if (SIM_MODE) {
    Serial.print("ANGLES joint: th1=");
    Serial.print(shoulder_joint_deg, 2);
    Serial.print(" th2=");
    Serial.print(elbow_joint_deg, 2);

    Serial.print(" | servo_cmd: s1=");
    Serial.print(shoulder_cmd, 2);
    Serial.print(" s2=");
    Serial.println(elbow_cmd, 2);
    return;
  }

  // REAL HARDWARE MODE
  shoulderServo.write(shoulder_cmd);
  elbowServo.write(elbow_cmd);
}


// -----------------------------
// IK for 2-link planar arm
// Returns true if reachable; outputs shoulder/elbow joint angles (deg)
// Elbow angle here is the "inner" joint angle from IK convention.
// -----------------------------
static bool ik2R(float x, float y, float &theta1_deg, float &theta2_deg) {
  float r2 = x*x + y*y;
  float c2 = (r2 - L1*L1 - L2*L2) / (2.0f * L1 * L2);

  if (c2 < -1.0f || c2 > 1.0f) {
    return false; // unreachable
  }

  // Choose elbow-down solution by default
  float theta2 = acosf(c2);                // radians
  float s2 = sinf(theta2);

  float k1 = L1 + L2 * c2;
  float k2 = L2 * s2;

  float theta1 = atan2f(y, x) - atan2f(k2, k1);

  theta1_deg = rad2deg(theta1);
  theta2_deg = rad2deg(theta2);

  return true;
}

// -----------------------------
// G-code execution
// -----------------------------
static void handleLine(String line) {
  line.trim();
  if (line.length() == 0) return;

  // Strip simple comments beginning with ';'
  int semi = line.indexOf(';');
  if (semi >= 0) line = line.substring(0, semi);
  line.trim();
  if (line.length() == 0) return;

  String u = line;
  u.toUpperCase();

  // MODE / UNITS
  if (u.indexOf("G90") >= 0) { mode = ABSOLUTE; Serial.println("ok"); return; }
  if (u.indexOf("G91") >= 0) { mode = RELATIVE; Serial.println("ok"); return; }
  if (u.indexOf("G20") >= 0) { units = INCH; Serial.println("ok"); return; }
  if (u.indexOf("G21") >= 0) { units = MM;   Serial.println("ok"); return; }

  // Program end / tool change (acknowledge)
  if (u.indexOf("M2") >= 0) { Serial.println("ok"); return; }
  if (u.indexOf("M6") >= 0) { Serial.println("ok"); return; }

  // MOTION: G0 or G1
  bool isG0 = (u.indexOf("G0") >= 0);
  bool isG1 = (u.indexOf("G1") >= 0);

  if (!(isG0 || isG1)) {
    Serial.println("error:unsupported");
    return;
  }

  // Parse X/Y/F
  bool fx=false, fy=false, ff=false;
  float X=0, Y=0, F=0;

  if (!parseWordFloat(u, 'X', fx, X) ||
      !parseWordFloat(u, 'Y', fy, Y) ||
      !parseWordFloat(u, 'F', ff, F)) {
    Serial.println("error:parse");
    return;
  }

  if (ff) feed = F;

  // If no X/Y provided, that's an error for our drawing robot
  // (You can relax this later if you want "move only X" etc.)
  if (!fx && !fy) {
    Serial.println("error:no_xy");
    return;
  }

  // Compute target based on ABS/REL, keep unspecified axis same
  float x_tgt = x_cur;
  float y_tgt = y_cur;

  float x_in = fx ? X : 0.0f;
  float y_in = fy ? Y : 0.0f;

  // Unit conversion
  if (units == INCH) {
    x_in = inchToMm(x_in);
    y_in = inchToMm(y_in);
  }

  if (mode == ABSOLUTE) {
    if (fx) x_tgt = x_in;
    if (fy) y_tgt = y_in;
  } else {
    if (fx) x_tgt = x_cur + x_in;
    if (fy) y_tgt = y_cur + y_in;
  }
  
  // DEBUG: show the final target (in mm) before IK
  Serial.print("TARGET mm: X=");
  Serial.print(x_tgt, 2);
  Serial.print(" Y=");
  Serial.println(y_tgt, 2);

  // IK
  float th1=0, th2=0;
  if (!ik2R(x_tgt, y_tgt, th1, th2)) {
    Serial.println("error:unreachable");
    return;
  }

  // Command servos
  writeServos(th1, th2);

  // Update current position
  x_cur = x_tgt;
  y_cur = y_tgt;

  Serial.println("ok");
}

void setup() {
  Serial.begin(115200);

  // Servo attach with standard pulse range
  shoulderServo.setPeriodHertz(50);
  elbowServo.setPeriodHertz(50);

  // You can adjust pulse range if your MG995 needs it, typical:
  // attach(pin, min_us, max_us)
  shoulderServo.attach(PIN_SHOULDER, 500, 2400);
  elbowServo.attach(PIN_ELBOW, 500, 2400);

  // Move to a safe "neutral"
  shoulderServo.write(90);
  elbowServo.write(90);

  Serial.println("ok"); // initial ready
}

void loop() {
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == '\n') {
      String line = lineBuf;
      lineBuf = "";
      handleLine(line);
    } else if (c != '\r') {
      // prevent runaway buffer growth
      if (lineBuf.length() < 200) lineBuf += c;
    }
  }
}