"""D455 adapter. Hardware ownership and raw acquisition remain on the board."""

import hashlib
import json
import logging
import math
import os
import queue
import re
import shutil
import threading
import time
import uuid
from contextlib import suppress
from datetime import UTC, datetime

import numpy as np
import pyrealsense2 as rs
from missioncore_plugin_sdk.v0alpha2.session import DeviceSessionSnapshot

MODEL = {
    "plugin_id": "missioncore.realsense",
    "plugin_version": "0.6.6",
    "model_id": "realsense.d455",
}


def utc():
    return datetime.now(UTC).isoformat()


def device_id(serial):
    return "rsd455_" + hashlib.sha256(serial.encode()).hexdigest()[:32]


def atomic(path, value):
    tmp = path.with_suffix(".tmp")
    with tmp.open("w") as f:
        json.dump(value, f, ensure_ascii=False, allow_nan=False)
        f.flush()
        os.fsync(f.fileno())
    tmp.replace(path)


def kind(profile):
    return str(profile.stream_type()).split(".")[-1]


def configure_profiles(config, profiles):
    for p in profiles:
        stream, fmt = getattr(rs.stream, p["stream"]), getattr(rs.format, p["format"])
        if "width" in p:
            config.enable_stream(stream, p["index"], p["width"], p["height"], fmt, p["fps"])
        else:
            config.enable_stream(stream, p["index"], fmt, p["fps"])


class Device:
    def __init__(self, serial, root, execution, usb_serial=None):
        self.serial = serial
        self.id = device_id(usb_serial or serial)
        self.root = root / self.id
        self.root.mkdir(exist_ok=True, mode=0o700)
        self.lock = threading.RLock()
        self.execution = execution
        self.session = "sensor_" + uuid.uuid4().hex
        self.opened = utc()
        self.revision = 0
        self.online = True
        self.acquisition = "idle"
        self.message = ""
        self.pipeline = None
        self.thread = None
        self.queue = queue.Queue(maxsize=2)
        self.stop_event = threading.Event()
        self.images = {}
        self.motion = {}
        self.depth = None
        self.intrinsics = None
        self.frames = {}
        self.last_frame = None
        self.record = None
        self.playback_id = None
        self.profiles = []
        self.options = []
        self.sdk_device = None
        self.firmware = ""
        self.transport = ""
        self.config = {"name": "RealSense D455", "verified": False}
        if (self.root / "config.json").exists():
            self.config.update(json.loads((self.root / "config.json").read_text()))
        # A previous verification is evidence, not a live readiness assertion.
        self.verified_this_process = False
        for path in self.root.glob("recordings/*/manifest.json"):
            value = json.loads(path.read_text())
            if value.get("state") == "recording":
                value.update(state="interrupted", recovered_at=utc())
                atomic(path, value)

    def save(self):
        atomic(self.root / "config.json", self.config)

    def refresh(self, dev):
        with self.lock:
            self.sdk_device = dev
            if not self.online:
                self.verified_this_process = False
                self.session = "sensor_" + uuid.uuid4().hex
                self.opened = utc()
            self.online = True
            self.firmware = dev.get_info(rs.camera_info.firmware_version)
            self.transport = dev.get_info(rs.camera_info.usb_type_descriptor)
            if self.pipeline is not None:
                return
            profiles, options = [], []
            for index, sensor in enumerate(dev.query_sensors()):
                for profile in sensor.get_stream_profiles():
                    stream = kind(profile)
                    if stream not in ("color", "depth", "infrared", "accel", "gyro"):
                        continue
                    data = {
                        "sensor": index,
                        "stream": stream,
                        "index": profile.stream_index(),
                        "fps": profile.fps(),
                        "format": str(profile.format()).split(".")[-1],
                    }
                    if profile.is_video_stream_profile():
                        video = profile.as_video_stream_profile()
                        data.update(width=video.width(), height=video.height())
                    data["id"] = hashlib.sha256(
                        json.dumps(data, sort_keys=True).encode()
                    ).hexdigest()[:16]
                    profiles.append(data)
                for option in sensor.get_supported_options():
                    try:
                        limits = sensor.get_option_range(option)
                        value = sensor.get_option(option)
                        if not all(
                            math.isfinite(x) for x in (limits.min, limits.max, limits.step, value)
                        ):
                            continue
                        options.append(
                            {
                                "id": f"{index}:{int(option)}",
                                "sensor": sensor.get_info(rs.camera_info.name),
                                "label": str(option).split(".")[-1],
                                "value": value,
                                "min": limits.min,
                                "max": limits.max,
                                "step": limits.step,
                                "read_only": sensor.is_option_read_only(option),
                            }
                        )
                    except RuntimeError:
                        continue
            self.profiles, self.options = profiles, options

    def defaults(self):
        result = []
        for stream, index, fmt, fps in [
            ("depth", 0, "z16", 15),
            ("color", 0, "rgb8", 15),
            ("infrared", 1, "y8", 15),
            ("infrared", 2, "y8", 15),
            ("accel", 0, "motion_xyz32f", 63),
            ("gyro", 0, "motion_xyz32f", 200),
        ]:
            candidates = [
                p
                for p in self.profiles
                if p["stream"] == stream and p["index"] == index and p["format"] == fmt
            ]
            if candidates:
                p = min(
                    candidates,
                    key=lambda p: (
                        abs(p.get("width", 640) - 640)
                        + abs(p.get("height", 480) - 480)
                        + abs(p["fps"] - fps) * 20
                    ),
                )
                result.append(p["id"])
        return result

    def snapshot(self, detailed=True):
        with self.lock:
            now = utc()
            snap = DeviceSessionSnapshot.model_validate(
                {
                    "context": {
                        "session_id": self.session,
                        "device": {
                            "device_id": self.id,
                            "model": MODEL,
                            "stability": "stable",
                            "basis": "hardware-identifier",
                        },
                        "execution": self.execution,
                        "opened_at": self.opened,
                    },
                    "revision": self.revision,
                    "enrollment": "enrolled" if self.config["verified"] else "empty",
                    "connectivity": "connected" if self.online else "offline",
                    "acquisition": self.acquisition,
                    "observed_at": now,
                    "message": self.message or None,
                }
            )
            value = {
                "id": self.id,
                "name": self.config["name"],
                "model": "RealSense D455",
                "prepared": True,
                "verified": self.verified_this_process,
                "online": self.online,
                "snapshot": snap.model_dump(mode="json"),
                "firmware": self.firmware,
                "usb": self.transport,
                "frames": dict(self.frames),
                "last_frame": self.last_frame,
                "recording": self.record,
                "playback_id": self.playback_id,
                "layers": list(self.images)
                + (["points"] if self.depth is not None else [])
                + (["motion"] if self.motion else []),
            }
            if detailed:
                value.update(
                    profiles=self.profiles,
                    defaults=self.defaults(),
                    options=self.options,
                    recordings=self.recordings(),
                    motion=dict(self.motion),
                )
            return value

    def callback(self, frame):
        if frame.is_motion_frame():
            v = frame.as_motion_frame().get_motion_data()
            key = kind(frame.profile)
            self.motion[key] = {
                "x": v.x,
                "y": v.y,
                "z": v.z,
                "timestamp_ms": frame.get_timestamp(),
                "clock": str(frame.get_frame_timestamp_domain()),
                "observed_at": utc(),
            }
            self.frames[key] = self.frames.get(key, 0) + 1
        elif frame.is_frameset():
            with suppress(queue.Full):
                self.queue.put_nowait(frame.as_frameset())

    def start(self, selected=None, record=False):
        with self.lock:
            if self.pipeline is not None:
                raise ValueError("Захват уже запущен. Сначала остановите его.")
            if not self.online or self.sdk_device is None:
                raise ValueError("Камера не подключена.")
            ids = selected if selected is not None else self.defaults()
            if not isinstance(ids, list) or not 1 <= len(ids) <= 6 or len(set(ids)) != len(ids):
                raise ValueError("Выберите профили потоков.")
            profiles = [next((p for p in self.profiles if p["id"] == ident), None) for ident in ids]
            if any(p is None for p in profiles) or len(
                {(p["stream"], p["index"]) for p in profiles}
            ) != len(profiles):
                raise ValueError("Выбраны несовместимые профили потоков.")
            if not any(p["stream"] in ("depth", "color", "infrared") for p in profiles):
                raise ValueError("Выберите хотя бы один видеопоток.")
            config = rs.config()
            config.enable_device(self.serial)
            configure_profiles(config, profiles)
            pipeline = rs.pipeline()
            if not config.can_resolve(rs.pipeline_wrapper(pipeline)):
                raise ValueError("Камера не поддерживает эту комбинацию профилей. Выберите другую.")
            self.playback_id = None
            self.acquisition = "starting"
            self.revision += 1
            self.images, self.motion, self.frames = {}, {}, {}
            self.depth, self.last_frame = None, None
            self.queue = queue.Queue(maxsize=2)
            self.stop_event.clear()
            record_path = None
            if record:
                if shutil.disk_usage(self.root).free < 2 * 1024**3:
                    self.acquisition = "idle"
                    raise ValueError("Для исходной записи нужно не меньше 2 ГиБ свободного места.")
                ident = "capture_" + uuid.uuid4().hex
                record_path = self.root / "recordings" / ident
                record_path.mkdir(parents=True, mode=0o700)
                config.enable_record_to_file(str(record_path / "source.db3"))
                self.record = {
                    "id": ident,
                    "state": "recording",
                    "started_at": utc(),
                    "monotonic_ns": time.monotonic_ns(),
                    "device_id": self.id,
                    "session_id": self.session,
                    "profiles": profiles,
                    "firmware": self.firmware,
                    "sdk": "2.58.4.10922",
                    "storage_format": "rosbag2-sqlite3",
                    "options": self.options,
                }
                atomic(record_path / "manifest.json", self.record)
            try:
                active = pipeline.start(config, self.callback)
                self.pipeline = pipeline
                self.acquisition = "streaming"
                self.message = ""
                self.depth_scale = active.get_device().first_depth_sensor().get_depth_scale()
                calibration = []
                for p in active.get_streams():
                    if p.is_video_stream_profile():
                        v = p.as_video_stream_profile().get_intrinsics()
                        calibration.append(
                            {
                                "stream": kind(p),
                                "index": p.stream_index(),
                                "width": v.width,
                                "height": v.height,
                                "fx": v.fx,
                                "fy": v.fy,
                                "ppx": v.ppx,
                                "ppy": v.ppy,
                                "coeffs": v.coeffs,
                                "model": str(v.model),
                            }
                        )
                if self.record:
                    self.record["calibration"] = calibration
                    self.record["depth_scale"] = self.depth_scale
                    atomic(record_path / "manifest.json", self.record)
                self.thread = threading.Thread(target=self.consume, daemon=True)
                self.thread.start()
            except Exception as error:
                logging.error("D455 capture: %s", str(error).replace(self.serial, "[camera]"))
                with suppress(RuntimeError):
                    pipeline.stop()
                self.pipeline = None
                self.acquisition = "failed"
                self.message = "Не удалось открыть потоки камеры. Проверьте подключение и профили."
                if self.record:
                    self.record.update(state="failed", ended_at=utc())
                    atomic(record_path / "manifest.json", self.record)
                    self.record = None
                raise ValueError(self.message) from None

    def replay(self, ident):
        # Only completed board-owned recordings; UI never supplies a filesystem path.
        if not isinstance(ident, str) or not re.fullmatch(r"capture_[0-9a-f]{32}", ident):
            raise ValueError("Некорректная запись.")
        with self.lock:
            if self.pipeline is not None:
                raise ValueError("Сначала остановите текущий захват или просмотр записи.")
            directory = self.root / "recordings" / ident
            source, manifest = directory / "source.db3", directory / "manifest.json"
            if directory.is_symlink() or source.is_symlink() or manifest.is_symlink():
                raise ValueError("Запись недоступна.")
            if not source.is_file() or not manifest.is_file():
                raise ValueError("Запись не найдена.")
            value = json.loads(manifest.read_text())
            if value.get("state") != "complete" or source.stat().st_size != value.get("bytes"):
                raise ValueError("Запись не завершена или повреждена.")
            config, pipeline = rs.config(), rs.pipeline()
            config.enable_device_from_file(str(source), repeat_playback=True)
            configure_profiles(config, value["profiles"])
            self.images, self.motion, self.frames = {}, {}, {}
            self.depth, self.last_frame = None, None
            self.queue = queue.Queue(maxsize=2)
            self.stop_event.clear()
            self.acquisition = "starting"
            try:
                active = pipeline.start(config, self.callback)
                self.pipeline = pipeline
                active.get_device().as_playback().set_real_time(True)
                self.depth_scale = active.get_device().first_depth_sensor().get_depth_scale()
                self.playback_id = ident
                self.acquisition = "streaming"
                self.message = ""
                self.thread = threading.Thread(target=self.consume, daemon=True)
                self.thread.start()
            except RuntimeError:
                with suppress(RuntimeError):
                    pipeline.stop()
                self.pipeline = None
                self.acquisition = "failed"
                raise ValueError("Не удалось открыть исходную запись.") from None
            self.revision += 1
            return {"ok": True, "playback_id": ident}

    def consume(self):
        colorizer = rs.colorizer()
        last_data, last_disk = time.monotonic(), time.monotonic()
        while not self.stop_event.is_set():
            try:
                frames = self.queue.get(timeout=1)
            except queue.Empty:
                if time.monotonic() - last_data > 8:
                    self.message = "Кадры перестали поступать. Проверьте USB и остановите захват."
                    self.acquisition = "failed"
                    self.online = False
                    break
                continue
            last_data = time.monotonic()
            for frame in frames:
                key = kind(frame.profile)
                if key == "infrared":
                    key += str(frame.profile.stream_index())
                if not frame.is_video_frame():
                    continue
                data = np.asanyarray(frame.get_data()).copy()
                if key == "depth":
                    self.depth = data
                    self.intrinsics = frame.profile.as_video_stream_profile().get_intrinsics()
                    data = np.asanyarray(colorizer.colorize(frame).get_data()).copy()
                elif data.ndim == 2:
                    data = np.repeat(data[:, :, None], 3, axis=2)
                elif frame.profile.format() == rs.format.bgr8:
                    data = data[:, :, ::-1].copy()
                elif frame.profile.format() != rs.format.rgb8:
                    continue
                self.images[key] = data
                self.frames[key] = self.frames.get(key, 0) + 1
                self.last_frame = {
                    "observed_at": utc(),
                    "monotonic_ns": time.monotonic_ns(),
                    "device_timestamp_ms": frame.get_timestamp(),
                    "clock": str(frame.get_frame_timestamp_domain()),
                }
            if self.record and time.monotonic() - last_disk > 2:
                last_disk = time.monotonic()
                if shutil.disk_usage(self.root).free < 512 * 1024**2:
                    self.message = "Запись остановлена: мало свободного места."
                    break
        if not self.stop_event.is_set():
            # Explicit device/disk failure closes raw recording; never auto-restart.
            self.stop(failed=True, from_capture=True)

    def stop(self, failed=False, from_capture=False):
        with self.lock:
            self.stop_event.set()
            pipeline, self.pipeline = self.pipeline, None
            if pipeline is not None:
                self.acquisition = "stopping"
                pipeline.stop()
                del pipeline
            if self.thread and not from_capture:
                self.thread.join(timeout=3)
            self.playback_id = None
            self.acquisition = "failed" if failed else "idle"
            self.revision += 1
            if self.record:
                value = dict(self.record)
                value.update(
                    state="failed" if failed else "complete",
                    ended_at=utc(),
                    ended_monotonic_ns=time.monotonic_ns(),
                    frames=dict(self.frames),
                )
                path = self.root / "recordings" / value["id"]
                source = path / "source.db3"
                if source.exists():
                    digest = hashlib.sha256()
                    with source.open("rb") as f:
                        for chunk in iter(lambda: f.read(1024 * 1024), b""):
                            digest.update(chunk)
                    value.update(sha256=digest.hexdigest(), bytes=source.stat().st_size)
                atomic(path / "manifest.json", value)
                self.record = None
            return {"ok": True}

    def verify(self):
        if self.pipeline is not None:
            raise ValueError("Остановите захват перед повторной проверкой.")
        try:
            self.start()
            deadline = time.monotonic() + 8
            expected = {
                p["stream"] + (str(p["index"]) if p["stream"] == "infrared" else "")
                for p in self.profiles
                if p["id"] in self.defaults()
            }
            while time.monotonic() < deadline:
                if all(self.frames.get(key, 0) >= 2 for key in expected):
                    self.config["verified"] = True
                    self.verified_this_process = True
                    self.config["verified_at"] = utc()
                    self.save()
                    return {
                        "ok": True,
                        "frames": dict(self.frames),
                        "verified_at": self.config["verified_at"],
                    }
                time.sleep(0.1)
            raise ValueError(
                "Не получены кадры всех выбранных потоков. Проверьте USB 3 и повторите проверку."
            )
        finally:
            self.stop()

    def rename(self, name):
        if (
            not isinstance(name, str)
            or not name.strip()
            or len(name) > 80
            or any(ord(c) < 32 for c in name)
        ):
            raise ValueError("Введите название до 80 символов.")
        with self.lock:
            self.config["name"] = name.strip()
            self.save()
            self.revision += 1
        return {"ok": True}

    def set_option(self, identifier, value):
        with self.lock:
            item = next((v for v in self.options if v["id"] == identifier), None)
            if (
                not item
                or item["read_only"]
                or type(value) not in (int, float)
                or not math.isfinite(value)
                or not item["min"] <= value <= item["max"]
            ):
                raise ValueError("Параметр недоступен или значение вне диапазона.")
            if self.playback_id:
                raise ValueError("Остановите просмотр записи перед настройкой камеры.")
            sensor_index, option_id = map(int, identifier.split(":"))
            sensor = self.sdk_device.query_sensors()[sensor_index]
            option = rs.option(option_id)
            sensor.set_option(option, float(value))
            item["value"] = sensor.get_option(option)
            self.revision += 1
            return {"ok": True, "value": item["value"]}

    def points(self):
        depth, intrinsics = self.depth, self.intrinsics
        if depth is None or intrinsics is None:
            return []
        # SDK deprojection respects the camera's actual distortion model.
        h, w = depth.shape
        stride = max(8, math.ceil(math.sqrt(h * w / 2000)))
        result = []
        for y in range(0, h, stride):
            for x in range(0, w, stride):
                z = float(depth[y, x]) * self.depth_scale
                if 0 < z < 15:
                    result.extend(
                        round(v, 4) for v in rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], z)
                    )
        return result

    def recordings(self):
        result = []
        for path in sorted(self.root.glob("recordings/*/manifest.json"), reverse=True)[:100]:
            value = json.loads(path.read_text())
            result.append(
                {
                    k: value.get(k)
                    for k in ("id", "state", "started_at", "ended_at", "bytes", "sha256")
                }
            )
        return result