fhd_scan_web_app/backend/routers/system.py

from __future__ import annotations

import asyncio
import time
from pathlib import Path

from fastapi import APIRouter, HTTPException, Request
from pydantic import BaseModel

import bag_metadata
import camera_params
import config
from ros_bridge import ROSBridge

router = APIRouter()


class SessionConfig(BaseModel):
    camera_count: int


@router.post("/session")
async def set_session(cfg: SessionConfig, request: Request):
    if cfg.camera_count not in config.CAMERA_PROFILES:
        raise HTTPException(400, f"unsupported camera_count, expected one of {sorted(config.CAMERA_PROFILES)}")
    session = request.app.state.session
    pm = request.app.state.pm
    if pm.is_running("lidar") or pm.is_running("camera"):
        raise HTTPException(409, "stop lidar/camera before changing camera_count")

    session.camera_count = cfg.camera_count
    # ROSBridge subscriptions are fixed at construction (see ros_bridge.py) —
    # changing camera_count means tearing down and recreating the bridge.
    old_ros: ROSBridge = request.app.state.ros
    old_ros.stop()
    new_ros = ROSBridge(camera_count=cfg.camera_count)
    new_ros.start()
    request.app.state.ros = new_ros
    return {"ok": True, "camera_count": session.camera_count}


def _camera_launch_cmd(app) -> str:
    session = app.state.session
    specs = config.camera_specs(session.camera_count)
    launch_file = config.camera_launch_file(session.camera_count)
    args = []
    for cam in specs:
        values = session.camera_values.get(cam.id) or camera_params.load_defaults(cam)
        session.camera_values[cam.id] = values
        path = camera_params.write_launch_params_yaml(cam, values)
        args.append(f" {cam.launch_arg}:={path}")
    return (
        f"{config.CAMERA_LAUNCH_SOURCE} && "
        f"stdbuf -oL -eL ros2 launch hik_camera_ros2_driver {launch_file}" + "".join(args)
    )


async def _auto_start_camera_after_delay(app):
    await asyncio.sleep(config.CAMERA_DELAY_SEC)
    pm = app.state.pm
    if pm.is_running("lidar") and not pm.is_running("camera"):
        pm.start("camera", _camera_launch_cmd(app), log_path=config.CAMERA_LOG_PATH)


@router.post("/lidar/start")
async def lidar_start(request: Request):
    pm = request.app.state.pm
    pm.start("lidar", config.LIDAR_LAUNCH_CMD, log_path=config.LIDAR_LOG_PATH)
    asyncio.create_task(_auto_start_camera_after_delay(request.app))
    return {"ok": True}


@router.post("/lidar/stop")
async def lidar_stop(request: Request):
    pm = request.app.state.pm
    session = request.app.state.session
    pm.stop("lidar")
    pm.stop("camera")
    # fastlio has no sensor data source once lidar stops — tear it down here
    # too, whether it's running because a recording is active or because the
    # operator started it standalone via the settings test panel.
    pm.stop("fastlio")
    if session.recording:
        pm.stop("recording")
        session.recording = False
        session.recording_path = None
        session.recording_started_at = None
    return {"ok": True}


@router.post("/camera/start")
async def camera_start(request: Request):
    pm = request.app.state.pm
    if not pm.is_running("lidar"):
        raise HTTPException(409, "start lidar first")
    pm.start("camera", _camera_launch_cmd(request.app), log_path=config.CAMERA_LOG_PATH)
    return {"ok": True}


@router.post("/camera/stop")
async def camera_stop(request: Request):
    request.app.state.pm.stop("camera")
    return {"ok": True}


@router.post("/fastlio/start")
async def fastlio_start(request: Request):
    """Manual FAST-LIO start, independent of recording — lets the operator
    preview lidar point-cloud quality (rig leveling, coverage) in Settings
    before committing to an actual recording."""
    pm = request.app.state.pm
    if not pm.is_running("lidar"):
        raise HTTPException(409, "start lidar first")
    request.app.state.ros.pointcloud.reset()
    pm.start("fastlio", config.FASTLIO_LAUNCH_CMD, log_path=config.FASTLIO_LOG_PATH, env=config.fastlio_env())
    return {"ok": True}


@router.post("/fastlio/stop")
async def fastlio_stop(request: Request):
    if request.app.state.session.recording:
        raise HTTPException(409, "recording is using fastlio — stop the recording instead")
    request.app.state.pm.stop("fastlio")
    return {"ok": True}


@router.post("/system/estop")
async def estop(request: Request):
    request.app.state.pm.stop_all()
    request.app.state.session.recording = False
    return {"ok": True}


def build_status(app) -> dict:
    from health import TopicThreshold

    pm = app.state.pm
    ros: ROSBridge = app.state.ros
    session = app.state.session
    specs = config.camera_specs(session.camera_count)

    lidar_running = pm.is_running("lidar")
    camera_running = pm.is_running("camera")
    fastlio_running = pm.is_running("fastlio")

    lidar_thr = config.HEALTH_THRESHOLDS["/livox/lidar"]
    cam_thr = config.HEALTH_THRESHOLDS["camera_image"]
    map_thr = config.HEALTH_THRESHOLDS["/Odometry"]

    cameras = []
    for cam in specs:
        health = ros.health.classify(
            cam.image_topic, camera_running, TopicThreshold(*cam_thr)
        )
        cameras.append({
            "id": cam.id, "topic": cam.image_topic, "health": health,
            "hz": ros.health.rate_hz(cam.image_topic),
        })

    recording_elapsed_s = None
    recording_size_bytes = None
    disk_check_dir = config.DEFAULT_SAVE_DIR
    if session.recording and session.recording_path:
        if session.recording_started_at is not None:
            recording_elapsed_s = round(time.monotonic() - session.recording_started_at, 1)
        recording_size_bytes = bag_metadata.dir_size_bytes(Path(session.recording_path))
        disk_check_dir = str(Path(session.recording_path).parent)

    lidar_health = ros.health.classify("/livox/lidar", lidar_running, TopicThreshold(*lidar_thr))

    return {
        "camera_count": session.camera_count,
        "default_save_dir": config.DEFAULT_SAVE_DIR,
        "lidar": {
            "running": lidar_running,
            "health": lidar_health,
            # /livox/imu isn't subscribed at all anymore (see ros_bridge.py's
            # module docstring — its ~200Hz starved the camera topics badly
            # enough that the operator asked to drop it, 2026-08-06). It
            # comes off the same livox_ros_driver2 process as the lidar
            # topic, so lidar's own health is a reasonable stand-in; there's
            # no real Hz number to report, so imu_hz stays 0 (the HUD badge
            # just shows the health word with no Hz suffix in that case).
            "imu_health": lidar_health,
            "hz": ros.health.rate_hz("/livox/lidar"),
            "imu_hz": 0.0,
        },
        "cameras": cameras,
        "map": {
            "health": ros.health.classify(config.ODOMETRY_TOPIC, fastlio_running, TopicThreshold(*map_thr)),
            "hz": ros.health.rate_hz(config.ODOMETRY_TOPIC),
            "points": ros.pointcloud.point_count(),
        },
        "recording": {
            "active": session.recording,
            "path": session.recording_path,
            "elapsed_s": recording_elapsed_s,
            "size_bytes": recording_size_bytes,
        },
        "disk": {
            "path": disk_check_dir,
            "free_gb": round(config.disk_free_gb(disk_check_dir), 2),
            "low_warning_gb": config.DISK_LOW_WARNING_GB,
            "low_danger_gb": config.DISK_LOW_DANGER_GB,
        },
        "process_status": pm.status(),
    }


@router.get("/status")
async def status(request: Request):
    return build_status(request.app)