# SPDX-License-Identifier: MIT
#
# Vendor-agnostic OPC UA <-> LinuxCNC bridge
#
# This script implements an industrial-style bridge between:
#   - Any OPC UA server (SCADA / PLC / HMI)
#   - LinuxCNC (via HAL + Python API)
#
# Features:
#   - Machine Enable / Disable and E-Stop control
#   - Real status feedback with ACK semantics
#   - Heartbeat and sequence counter supervision
#   - Recipe / Cycle execution via MDI (O<sub> call [id])
#   - BUSY / DONE / ERROR / ERROR_CODE reporting
#
# Vendor neutrality:
#   - This script is OPC UA vendor-agnostic.
#   - It does NOT depend on any Mitsubishi-, Ignition-, Kepware- or Siemens-specific API.
#   - It only requires a standard OPC UA server.
#
# Tested with:
#   - Mitsubishi OPC UA Server (MELSOFT / iQ platform)
#   - LinuxCNC 2.10 (uspace)
#   - LinuxCNC AXIS simulator (Stepconf-generated config) for integration validation
#   - Debian 12 (Bookworm)
#
# Copyright (c) 2026
#
# This file is licensed under the MIT License.
# See the LICENSE file in the project root for the full license text.

import os
import sys
import time
import subprocess
import threading
from typing import Tuple, List

from opcua import Client, ua


# =========================
# LinuxCNC binding loader
# =========================
def _try_import_linuxcnc() -> Tuple[object, List[str]]:
    """
    Returns (linuxcnc_module, attempted_paths).
    If linuxcnc is a namespace-only empty package in the venv, tries to add
    common system paths and re-import.
    """
    attempted: List[str] = []

    def is_real_linuxcnc(mod) -> bool:
        return hasattr(mod, "command") and hasattr(mod, "stat")

    # 1) First attempt: whatever Python finds
    try:
        import linuxcnc  # type: ignore
        if is_real_linuxcnc(linuxcnc):
            return linuxcnc, attempted
    except Exception:
        pass

    # 2) If we got here, either import failed OR it imported an empty namespace package.
    # Add typical LinuxCNC python install locations.
    pyver = f"python{sys.version_info.major}.{sys.version_info.minor}"
    candidates = [
        "/usr/lib/python3/dist-packages",                       # Debian/Ubuntu dist-packages
        f"/usr/local/lib/{pyver}/dist-packages",               # /usr/local dist-packages
        f"/usr/local/lib/{pyver}/site-packages",               # /usr/local site-packages
        f"/usr/lib/{pyver}/dist-packages",                     # less common
        f"/usr/lib/{pyver}/site-packages",                     # less common
    ]

    # Also respect an override env var if you ever want:
    #   export LCNC_PYTHONPATH=/path/to/linuxcnc/python
    override = os.environ.get("LCNC_PYTHONPATH")
    if override:
        candidates.insert(0, override)

    for p in candidates:
        if p and os.path.isdir(p) and p not in sys.path:
            sys.path.insert(0, p)
            attempted.append(p)

    # Re-import after path injection
    try:
        import importlib
        linuxcnc = importlib.import_module("linuxcnc")  # type: ignore
        linuxcnc = importlib.reload(linuxcnc)           # type: ignore
        if is_real_linuxcnc(linuxcnc):
            return linuxcnc, attempted
    except Exception:
        pass

    # Still not real -> return whatever we have (or raise)
    try:
        import linuxcnc  # type: ignore
        return linuxcnc, attempted
    except Exception as e:
        raise ImportError(f"Failed to import linuxcnc after trying paths {attempted}: {e!r}") from e


LINUXCNC, _LCNC_PATHS_TRIED = _try_import_linuxcnc()

# Validate
if not (hasattr(LINUXCNC, "command") and hasattr(LINUXCNC, "stat")):
    raise ImportError(
        "LinuxCNC python binding not found (linuxcnc.command/stat missing). "
        f"Tried adding paths: {_LCNC_PATHS_TRIED}. "
        "This usually means the linuxcnc Python module is not installed for this Python version "
        "or is installed in a non-standard location. "
        "If you know the path, set env var LCNC_PYTHONPATH to that directory."
    )


# =========================
# OPC UA endpoint (vendor-agnostic)
# =========================
URL = "opc.tcp://192.168.3.12:48010"

# =========================
# NodeIds (vendor-agnostic tags)
# =========================

# Enable (Machine On/Off)
N_STATUS_ENABLE = "ns=2;s=Studio.Tags.Application.LCNC_STATUS"
N_CMD_ENABLE    = "ns=2;s=Studio.Tags.Application.LCNC_CMD_ENABLE"
N_ACK_ENABLE    = "ns=2;s=Studio.Tags.Application.LCNC_CMD_ENABLE_ACK"

# E-Stop
N_STATUS_ESTOP  = "ns=2;s=Studio.Tags.Application.LCNC_STATUS_ESTOP"
N_CMD_ESTOP     = "ns=2;s=Studio.Tags.Application.LCNC_CMD_ESTOP"

# Heartbeat (visual)
N_HEARTBEAT     = "ns=2;s=Studio.Tags.Application.LCNC_HEARTBEAT"

# Robust heartbeat: Int32 sequence counter
N_HB_SEQ        = "ns=2;s=Studio.Tags.Application.LCNC_HB_SEQ"

# ===== Recipe / Cycle =====
N_RECIPE_ID         = "ns=2;s=Studio.Tags.Application.LCNC_RECIPE_ID"          # Int32
N_RECIPE_START_SEQ  = "ns=2;s=Studio.Tags.Application.LCNC_RECIPE_START_SEQ"   # Int32
N_RECIPE_ABORT      = "ns=2;s=Studio.Tags.Application.LCNC_RECIPE_ABORT"       # Bool

# States
N_CYCLE_BUSY        = "ns=2;s=Studio.Tags.Application.LCNC_CYCLE_BUSY"         # Bool
N_CYCLE_DONE        = "ns=2;s=Studio.Tags.Application.LCNC_CYCLE_DONE"         # Bool (pulse)
N_CYCLE_ERROR       = "ns=2;s=Studio.Tags.Application.LCNC_CYCLE_ERROR"        # Bool (latch)
N_ACTIVE_RECIPE_ID  = "ns=2;s=Studio.Tags.Application.LCNC_ACTIVE_RECIPE_ID"   # Int32
N_ERROR_CODE        = "ns=2;s=Studio.Tags.Application.LCNC_ERROR_CODE"         # Int32

# =========================
# Timings
# =========================
POLL_S = 0.2          # 200 ms
PULSE_S = 0.25
HB_PERIOD_S = 0.5
HB_SEQ_PERIOD_S = 1.0
DONE_PULSE_S = 0.5
DBG_PERIOD_S = 2.0

# =========================
# Cycle (real) via MDI
# =========================
RECIPE_SUB_NAME = "recipe_runner"

# =========================
# Error codes
# =========================
EC_ABORT                 = 200
EC_START_REJECT_ESTOP    = 101
EC_START_REJECT_ENABLE   = 102
EC_ESTOP_DURING_CYCLE    = 103
EC_MDI_FAIL              = 300
EC_START_IGNORED_RUNNING = 110


# -------------------------
# HAL helpers
# -------------------------
def hal_get_bool(pin: str) -> bool:
    out = subprocess.check_output(["halcmd", "-s", "show", "pin", pin], text=True)
    return " TRUE " in f" {out} "


def hal_pulse(pin: str, seconds: float = PULSE_S) -> None:
    subprocess.check_call(["halcmd", "setp", pin, "TRUE"])
    time.sleep(seconds)
    subprocess.check_call(["halcmd", "setp", pin, "FALSE"])


# -------------------------
# OPC UA helpers (Value-only writes)
# -------------------------
def write_bool_value_only(node, value: bool) -> None:
    dv = ua.DataValue()
    dv.Value = ua.Variant(bool(value), ua.VariantType.Boolean)
    node.set_attribute(ua.AttributeIds.Value, dv)


def write_i32_value_only(node, value: int) -> None:
    dv = ua.DataValue()
    dv.Value = ua.Variant(int(value), ua.VariantType.Int32)
    node.set_attribute(ua.AttributeIds.Value, dv)


def read_bool(node) -> bool:
    return bool(node.get_value())


def read_i32(node) -> int:
    return int(node.get_value())


def main() -> None:
    # LinuxCNC API objects (now from the real binding)
    lcmd = LINUXCNC.command()
    lstat = LINUXCNC.stat()

    client = Client(URL)
    client.connect()

    # Nodes
    n_status_enable = client.get_node(N_STATUS_ENABLE)
    n_cmd_enable    = client.get_node(N_CMD_ENABLE)
    n_ack_enable    = client.get_node(N_ACK_ENABLE)

    n_status_estop  = client.get_node(N_STATUS_ESTOP)
    n_cmd_estop     = client.get_node(N_CMD_ESTOP)

    n_hb            = client.get_node(N_HEARTBEAT)
    n_hb_seq        = client.get_node(N_HB_SEQ)

    n_recipe_id        = client.get_node(N_RECIPE_ID)
    n_recipe_start_seq = client.get_node(N_RECIPE_START_SEQ)
    n_recipe_abort     = client.get_node(N_RECIPE_ABORT)

    n_cycle_busy       = client.get_node(N_CYCLE_BUSY)
    n_cycle_done       = client.get_node(N_CYCLE_DONE)
    n_cycle_error      = client.get_node(N_CYCLE_ERROR)
    n_active_recipe_id = client.get_node(N_ACTIVE_RECIPE_ID)
    n_error_code       = client.get_node(N_ERROR_CODE)

    # Track last
    last_status_enable = None
    last_status_estop  = None
    last_cmd_enable    = None
    last_cmd_estop     = None

    # Heartbeats
    hb_value = False
    hb_seq = 0
    next_hb_t = time.monotonic() + HB_PERIOD_S
    next_hb_seq_t = time.monotonic() + HB_SEQ_PERIOD_S
    next_dbg_t = time.monotonic() + DBG_PERIOD_S

    # Inputs state
    last_abort = False
    last_start_seq = None

    # DONE pulse
    done_pulse_until = 0.0

    # Cycle state
    cycle_lock = threading.Lock()
    cycle_running = False

    def set_cycle_outputs(*, busy=None, done=None, err=None, arid=None, ec=None) -> None:
        try:
            if busy is not None:
                write_bool_value_only(n_cycle_busy, bool(busy))
            if done is not None:
                write_bool_value_only(n_cycle_done, bool(done))
            if err is not None:
                write_bool_value_only(n_cycle_error, bool(err))
            if arid is not None:
                write_i32_value_only(n_active_recipe_id, int(arid))
            if ec is not None:
                write_i32_value_only(n_error_code, int(ec))
        except Exception as e:
            print("ERROR writing cycle outputs:", repr(e))

    def cycle_thread_fn(recipe_id: int) -> None:
        nonlocal cycle_running, done_pulse_until
        mdi_line = f"O<{RECIPE_SUB_NAME}> call [{int(recipe_id)}]"
        print("MDI start:", mdi_line)

        try:
            lcmd.mode(LINUXCNC.MODE_MDI)
            lcmd.wait_complete()

            lcmd.mdi(mdi_line)
            lcmd.wait_complete()

            # Wait for interpreter to become idle
            while True:
                time.sleep(0.05)
                lstat.poll()

                with cycle_lock:
                    if not cycle_running:
                        return

                if hasattr(LINUXCNC, "INTERP_IDLE"):
                    if lstat.interp_state == LINUXCNC.INTERP_IDLE:
                        break
                else:
                    # Fallback heuristic
                    if getattr(lstat, "motion_line", 0) == 0:
                        break

            with cycle_lock:
                cycle_running = False

            set_cycle_outputs(busy=False, err=False, ec=0)
            done_pulse_until = time.monotonic() + DONE_PULSE_S
            set_cycle_outputs(done=True)
            print("Cycle completed OK (MDI).")

        except Exception as e:
            print("ERROR MDI cycle:", repr(e))
            with cycle_lock:
                cycle_running = False
            set_cycle_outputs(busy=False, err=True, ec=EC_MDI_FAIL)

    # Init outputs
    set_cycle_outputs(busy=False, done=False, err=False, arid=0, ec=0)

    print("opcbridge_main running (Enable/E-Stop + HB_SEQ + START_SEQ + REAL MDI cycle). Ctrl+C to exit.")
    if _LCNC_PATHS_TRIED:
        print("linuxcnc binding loaded after adding paths:", _LCNC_PATHS_TRIED)

    try:
        while True:
            now = time.monotonic()

            # Heartbeats
            if now >= next_hb_t:
                hb_value = not hb_value
                try:
                    write_bool_value_only(n_hb, hb_value)
                except Exception as e:
                    print("ERROR writing HEARTBEAT:", repr(e))
                next_hb_t = now + HB_PERIOD_S

            if now >= next_hb_seq_t:
                hb_seq = (hb_seq + 1) & 0x7FFFFFFF
                try:
                    write_i32_value_only(n_hb_seq, hb_seq)
                except Exception as e:
                    print("ERROR writing HB_SEQ:", repr(e))
                next_hb_seq_t = now + HB_SEQ_PERIOD_S

            # Status LinuxCNC -> OPC
            cur_status_enable = hal_get_bool("motion.motion-enabled")
            if cur_status_enable != last_status_enable:
                try:
                    write_bool_value_only(n_status_enable, cur_status_enable)
                except Exception as e:
                    print("ERROR writing LCNC_STATUS:", repr(e))
                print("LCNC_STATUS (enable) =", cur_status_enable)
                last_status_enable = cur_status_enable

            cur_status_estop = hal_get_bool("halui.estop.is-activated")
            if cur_status_estop != last_status_estop:
                try:
                    write_bool_value_only(n_status_estop, cur_status_estop)
                except Exception as e:
                    print("ERROR writing LCNC_STATUS_ESTOP:", repr(e))
                print("LCNC_STATUS_ESTOP =", cur_status_estop)
                last_status_estop = cur_status_estop

            # ACK = STATUS
            try:
                write_bool_value_only(n_ack_enable, cur_status_enable)
            except Exception as e:
                print("ERROR writing LCNC_CMD_ENABLE_ACK:", repr(e))

            # Commands OPC -> LinuxCNC
            cur_cmd_estop = read_bool(n_cmd_estop)
            if cur_cmd_estop != last_cmd_estop:
                print("LCNC_CMD_ESTOP changed to:", cur_cmd_estop)
                if cur_cmd_estop:
                    hal_pulse("halui.estop.activate", PULSE_S)
                else:
                    hal_pulse("halui.estop.reset", PULSE_S)
                last_cmd_estop = cur_cmd_estop

            cur_cmd_enable = read_bool(n_cmd_enable)
            if cur_cmd_enable != last_cmd_enable:
                print("LCNC_CMD_ENABLE changed to:", cur_cmd_enable)
                if cur_cmd_enable:
                    if not cur_cmd_estop:
                        hal_pulse("halui.estop.reset", PULSE_S)
                        hal_pulse("halui.machine.on", PULSE_S)
                    else:
                        print("Enable requested, but CMD_ESTOP=TRUE. Ignoring machine.on.")
                else:
                    hal_pulse("halui.machine.off", PULSE_S)
                last_cmd_enable = cur_cmd_enable

            # If E-STOP becomes active during cycle: abort
            with cycle_lock:
                running_snapshot = cycle_running
            if running_snapshot and cur_status_estop:
                print("E-STOP during cycle: aborting.")
                try:
                    lcmd.abort()
                except Exception as e:
                    print("WARN linuxcnc abort error:", repr(e))
                with cycle_lock:
                    cycle_running = False
                set_cycle_outputs(busy=False, err=True, ec=EC_ESTOP_DURING_CYCLE)

            # Recipe / Cycle protocol
            try:
                recipe_id = read_i32(n_recipe_id)
            except Exception:
                recipe_id = 0

            try:
                start_seq = read_i32(n_recipe_start_seq)
            except Exception:
                start_seq = 0

            try:
                abort_level = read_bool(n_recipe_abort)
            except Exception:
                abort_level = False

            abort_rise = (not last_abort) and abort_level
            last_abort = abort_level

            start_seq_changed = False
            if last_start_seq is None:
                last_start_seq = start_seq
            elif start_seq != last_start_seq:
                start_seq_changed = True
                last_start_seq = start_seq

            # ABORT
            if abort_rise:
                print("RECIPE_ABORT rising edge.")
                try:
                    lcmd.abort()
                except Exception as e:
                    print("WARN linuxcnc abort error:", repr(e))

                with cycle_lock:
                    cycle_running = False

                set_cycle_outputs(busy=False, done=False, err=True, arid=recipe_id, ec=EC_ABORT)

            # START (SEQ)
            if start_seq_changed:
                print(f"RECIPE_START event (SEQ): RECIPE_ID={recipe_id} START_SEQ={start_seq}")
                set_cycle_outputs(done=False, err=False, ec=0)

                if cur_status_estop:
                    print("START rejected: E-STOP active.")
                    set_cycle_outputs(busy=False, err=True, arid=0, ec=EC_START_REJECT_ESTOP)

                elif not cur_status_enable:
                    print("START rejected: machine not enabled (Enable=0).")
                    set_cycle_outputs(busy=False, err=True, arid=0, ec=EC_START_REJECT_ENABLE)

                else:
                    with cycle_lock:
                        if cycle_running:
                            print("START ignored: cycle already running.")
                            set_cycle_outputs(err=True, ec=EC_START_IGNORED_RUNNING)
                        else:
                            cycle_running = True

                    with cycle_lock:
                        if cycle_running:
                            set_cycle_outputs(busy=True, done=False, err=False, arid=recipe_id, ec=0)
                            t = threading.Thread(target=cycle_thread_fn, args=(int(recipe_id),), daemon=True)
                            t.start()

            # DONE pulse end
            if done_pulse_until > 0.0 and now >= done_pulse_until:
                done_pulse_until = 0.0
                set_cycle_outputs(done=False)

            # Debug
            if now >= next_dbg_t:
                try:
                    srv_status = bool(n_status_enable.get_value())
                    srv_ack    = bool(n_ack_enable.get_value())
                    srv_estop  = bool(n_status_estop.get_value())
                    srv_hb_seq = int(n_hb_seq.get_value())
                    srv_busy   = bool(n_cycle_busy.get_value())
                    srv_done   = bool(n_cycle_done.get_value())
                    srv_err    = bool(n_cycle_error.get_value())
                    srv_arid   = int(n_active_recipe_id.get_value())
                    srv_ec     = int(n_error_code.get_value())
                    print(
                        "DBG readback:"
                        f" STATUS={srv_status} ACK={srv_ack} ESTOP={srv_estop} HB_SEQ={srv_hb_seq}"
                        f" | BUSY={srv_busy} DONE={srv_done} ERR={srv_err} ARID={srv_arid} EC={srv_ec}"
                        f" | START_SEQ={start_seq}"
                    )
                except Exception as e:
                    print("DBG readback error:", repr(e))
                next_dbg_t = now + DBG_PERIOD_S

            time.sleep(POLL_S)

    finally:
        client.disconnect()


if __name__ == "__main__":
    main()
