#!/usr/bin/env python3
"""Small D640 ROS workflow. No camera is opened by help, doctor or --show-config."""
import argparse
import importlib.util
import json
import math
import os
from pathlib import Path
import re
import shutil
import subprocess
import sys

PACKAGE = "maixsense_d640_driver"


def package_share():
    try:
        from ament_index_python.packages import get_package_share_directory
        return Path(get_package_share_directory(PACKAGE))
    except (ImportError, LookupError):
        script = Path(__file__).resolve()
        for candidate in (script.parent.parent, script.parents[2] / "share" / PACKAGE):
            if (candidate / "launch" / "_workflow.py").is_file():
                return candidate
        raise RuntimeError("D640 package is unavailable; source your installation's env.bash")


def workflow():
    spec = importlib.util.spec_from_file_location("d640_cli_workflow", package_share() / "launch" / "_workflow.py")
    module = importlib.util.module_from_spec(spec)
    spec.loader.exec_module(module)
    return module


def namespace(value):
    result = value.strip("/")
    if result and not re.fullmatch(r"[A-Za-z_][A-Za-z0-9_]*(/[A-Za-z_][A-Za-z0-9_]*)*", result):
        raise argparse.ArgumentTypeError("namespace must contain ROS names separated by single '/' characters")
    return result


def parse_set(values):
    result = {}
    for assignment in values:
        name, separator, raw = assignment.partition("=")
        if not separator or not re.fullmatch(r"[A-Za-z_][A-Za-z0-9_]*", name):
            raise ValueError("--set requires KEY=VALUE with a scalar ROS parameter")
        try:
            import yaml
            try:
                value = yaml.safe_load(raw)
            except yaml.YAMLError as error:
                raise ValueError("invalid --set YAML scalar: " + str(error)) from None
        except ImportError:
            raise RuntimeError("--set requires python3-yaml; install it and use the system Python") from None
        if type(value) not in (bool, int, float, str) or (isinstance(value, float) and not math.isfinite(value)):
            raise ValueError("--set supports only finite scalar values; quote string values explicitly")
        result[name] = value
    return result


def doctor():
    failures = []
    def check(ok, label, detail):
        print(("[OK]   " if ok else "[FAIL] ") + label + ": " + detail)
        if not ok:
            failures.append(label)
    check(shutil.which("ros2") is not None, "ROS CLI", shutil.which("ros2") or "not on PATH; source <installation>/env.bash")
    check(bool(os.environ.get("AMENT_PREFIX_PATH")), "ROS environment", os.environ.get("ROS_DISTRO", "not loaded; source <installation>/env.bash"))
    try:
        from ament_index_python.packages import get_package_prefix
        prefix = Path(get_package_prefix(PACKAGE))
        executable = prefix / "lib" / PACKAGE / PACKAGE
        check(executable.is_file() and os.access(executable, os.X_OK), "driver executable", str(executable))
        if executable.is_file() and shutil.which("ldd"):
            libraries = subprocess.run(["ldd", str(executable)], capture_output=True, text=True, timeout=8)
            check(libraries.returncode == 0 and "not found" not in libraries.stdout,
                  "runtime libraries", "all resolved" if libraries.returncode == 0 and "not found" not in libraries.stdout else libraries.stdout.strip() or libraries.stderr.strip())
        share = package_share()
        check((share / "config" / "driver.yaml").is_file(), "default configuration", str(share / "config" / "driver.yaml"))
        try:
            get_package_prefix("rviz2")
            print("[OK]   RViz: available")
        except LookupError:
            print("[INFO] RViz: missing; start works, view needs ros-humble-rviz2")
        for dependency in ("rclpy", "ros2launch", "ros2bag", "rosbag2_transport"):
            try:
                get_package_prefix(dependency)
                check(True, dependency, "available")
            except LookupError:
                check(False, dependency, "missing; install ros-humble-" + dependency.replace("_", "-"))
        check(importlib.util.find_spec("yaml") is not None, "YAML", "python3-yaml required for launch presets and RViz")
        try:
            importlib.import_module("rclpy")  # Import only: no context, node, daemon or device.
            check(True, "ROS Python ABI", sys.executable)
        except (ImportError, OSError, RuntimeError) as error:
            check(False, "ROS Python ABI", "cannot import rclpy with " + sys.executable +
                  "; deactivate Conda/use system Python, then source <installation>/env.bash: " + str(error))
        plugin = prefix / "lib" / "libmaixsense_d640_component.so"
        print("[INFO] component: " + ("available" if plugin.is_file() else "optional, not built"))
    except (ImportError, LookupError):
        check(False, "D640 ROS package", "not in the active overlay; source <installation>/env.bash")
    devices = []
    for entry in sorted(Path("/sys/bus/usb/devices").glob("*")):
        try:
            if (entry / "idVendor").read_text().strip() == "359f" and (entry / "idProduct").read_text().strip() == "4301":
                devices.append((entry / "serial").read_text().strip())
        except OSError:
            pass
    print("[INFO] USB D640 identities: " + (", ".join(devices) if devices else "none detected (connect the camera before start/view)"))
    print("Doctor reads environment, package files, shared-library metadata and USB identity files only; it does not start a node, daemon, stream or device control.")
    return 1 if failures else 0


def call_trigger(ns, operation, timeout=10):
    try:
        import rclpy
        from std_srvs.srv import Trigger
    except ImportError:
        raise RuntimeError("ROS Python is unavailable; source <installation>/env.bash") from None
    rclpy.init(args=[])
    node = rclpy.create_node("d640_workflow_client")
    try:
        service = "/" + "/".join(filter(None, (ns, operation)))
        client = node.create_client(Trigger, service)
        if not client.wait_for_service(timeout_sec=3.0):
            raise RuntimeError(service + " is unavailable; start the camera first (check --namespace)")
        future = client.call_async(Trigger.Request())
        rclpy.spin_until_future_complete(node, future, timeout_sec=float(timeout))
        if not future.done() or future.result() is None:
            raise RuntimeError(service + " timed out")
        response = future.result()
        try:
            print(json.dumps(json.loads(response.message), ensure_ascii=False, indent=2))
        except ValueError:
            print(response.message)
        return 0 if response.success else 1
    finally:
        node.destroy_node()
        rclpy.shutdown()


def exec_ros2(arguments):
    executable = shutil.which("ros2")
    if not executable:
        raise RuntimeError("ros2 not found; source <installation>/env.bash")
    os.execv(executable, [executable] + arguments)


def main():
    parser = argparse.ArgumentParser(description="D640: doctor -> view/start -> status -> save/record")
    commands = parser.add_subparsers(dest="command")
    commands.add_parser("doctor", help="read-only environment and installation diagnosis")
    for operation in ("start", "view"):
        command = commands.add_parser(operation, help="start camera" + (" with matching RViz" if operation == "view" else " without a GUI"))
        command.add_argument("--preset", choices=("default", "depth", "ir", "pseudo"), default="default")
        command.add_argument("--serial", default=None, help="omit to preserve YAML; empty string selects first camera")
        command.add_argument("--namespace", type=namespace, default="stereo")
        command.add_argument("--frame-prefix", default=None)
        command.add_argument("--params", type=Path)
        command.add_argument("--set", action="append", default=[], metavar="KEY=VALUE", help="explicit scalar override, repeatable")
        command.add_argument("--show-config", action="store_true", help="print resolved overrides without launching or opening the camera")
    command = commands.add_parser("status", help="read actual device configuration from a running camera")
    command.add_argument("--namespace", type=namespace, default="stereo")
    command = commands.add_parser("save", help="save recent images and/or XYZ PLY using the running driver")
    command.add_argument("--kind", choices=("images", "cloud", "both"), default="both")
    command.add_argument("--namespace", type=namespace, default="stereo")
    command = commands.add_parser("record", help="record standard ROS topics using rosbag2; Ctrl+C stops recording")
    command.add_argument("--output", required=True, type=Path)
    command.add_argument("--namespace", type=namespace, default="stereo")
    args = parser.parse_args()
    if args.command is None:
        parser.print_help()
        return 0
    if args.command == "doctor":
        return doctor()
    if args.command in ("start", "view"):
        overrides = parse_set(args.set)
        if args.params is not None and not args.params.is_file():
            raise ValueError("parameter file does not exist: " + str(args.params))
        if args.show_config:
            helper = workflow()
            source = args.params or package_share() / "config" / "driver.yaml"
            named = {"serial": helper.YAML_DEFAULT if args.serial is None else args.serial,
                     "frame_prefix": helper.YAML_DEFAULT if args.frame_prefix is None else args.frame_prefix}
            values = helper.effective_view_parameters(source, args.namespace, args.preset,
                json.dumps(overrides, allow_nan=False), **named)
            print(json.dumps({"namespace": "/" + args.namespace if args.namespace else "/",
                "node": "/" + "/".join(filter(None, (args.namespace, "camera"))),
                "parameters_file": str(source.resolve()), "effective_overrides": values,
                "rviz": helper.rviz_config(args.namespace, values) if args.command == "view" else None}, ensure_ascii=False, indent=2))
            return 0
        arguments = ["launch", PACKAGE, "view.launch.py" if args.command == "view" else "driver.launch.py",
                     "camera_namespace:=" + args.namespace, "preset:=" + args.preset,
                     "overrides:=" + json.dumps(overrides, allow_nan=False)]
        if args.serial is not None:
            arguments.append("serial:=" + args.serial)
        if args.frame_prefix is not None:
            arguments.append("frame_prefix:=" + args.frame_prefix)
        if args.params is not None:
            arguments.append("parameters_file:=" + str(args.params.resolve()))
        exec_ros2(arguments)
    if args.command == "status":
        return call_trigger(args.namespace, "get_device_config")
    if args.command == "save":
        result = 0
        if args.kind in ("images", "both"):
            result |= call_trigger(args.namespace, "save_images")
        if args.kind in ("cloud", "both"):
            result |= call_trigger(args.namespace, "save_point_cloud")
        return result
    if args.command == "record":
        output = args.output.expanduser().resolve()
        if output.exists():
            raise ValueError("record output already exists: " + str(output))
        suffixes = ["depth/image_raw", "depth/camera_info", "left/image_raw", "left/camera_info",
                    "right/image_raw", "right/camera_info", "pseudo_color/image_raw", "pseudo_color/camera_info",
                    "depth/metadata", "left/metadata", "right/metadata", "pseudo_color/metadata",
                    "points2", "left/image_rect", "left/camera_info_rect", "right/image_rect", "right/camera_info_rect",
                    "depth/disparity", "left/image_raw/compressed", "right/image_raw/compressed", "pseudo_color/image_raw/compressed",
                    "gyro_accel/sample", "gyro/sample", "accel/sample", "temperature", "diagnostics", "device_status"]
        topics = ["/" + "/".join(filter(None, (args.namespace, suffix))) for suffix in suffixes]
        exec_ros2(["bag", "record", "-o", str(output)] + topics + ["/tf_static"])
    return 0


if __name__ == "__main__":
    try:
        sys.exit(main())
    except (OSError, RuntimeError, ValueError, subprocess.TimeoutExpired) as error:
        print("d640: " + str(error), file=sys.stderr)
        sys.exit(1)
