"""LiDAR collector emulator executable compatible with Synapse LidarRunner."""

from __future__ import annotations

import argparse
import csv
import logging
import math
import os
import signal
import sys
import threading
import time

logger = logging.getLogger(__name__)


def _build_parser() -> argparse.ArgumentParser:
    parser = argparse.ArgumentParser(description="Synapse-compatible LiDAR collector emulator")
    parser.add_argument("config_path", nargs="?", help="Collector config path (unused, accepted for compatibility)")
    parser.add_argument("--rate-hz", type=float, default=20.0, help="Output row rate")
    parser.add_argument("--samples-per-cycle", type=int, default=32, help="Point rows per cycle")
    return parser


def _require_env_path(name: str) -> str:
    value = os.environ.get(name)
    if not value:
        raise ValueError(f"Missing required environment variable: {name}")
    return value


def main(argv: list[str] | None = None) -> int:
    logging.basicConfig(
        level=logging.INFO,
        format="%(asctime)s %(levelname)s [%(name)s] %(message)s",
    )

    parser = _build_parser()
    args = parser.parse_args(argv)

    try:
        point_path = _require_env_path("POINT_CLOUD_CSV_PATH")
        imu_path = _require_env_path("IMU_DATA_CSV_PATH")
    except ValueError as exc:
        logger.error("%s", exc)
        return 2

    os.makedirs(os.path.dirname(point_path) or ".", exist_ok=True)
    os.makedirs(os.path.dirname(imu_path) or ".", exist_ok=True)

    stop_event = threading.Event()

    def _request_stop(signum, frame):  # type: ignore[no-untyped-def]
        logger.info("Received signal %s, shutting down LiDAR collector emulator", signum)
        stop_event.set()

    signal.signal(signal.SIGTERM, _request_stop)
    signal.signal(signal.SIGINT, _request_stop)

    period_s = 1.0 / max(args.rate_hz, 0.1)
    point_id = 0
    start = time.monotonic()

    logger.info("LiDAR collector emulator started (config_path=%s)", args.config_path)
    logger.info("Writing point cloud data to %s", point_path)
    logger.info("Writing IMU data to %s", imu_path)

    try:
        with open(point_path, "w", newline="", encoding="utf-8") as point_file, open(
            imu_path,
            "w",
            newline="",
            encoding="utf-8",
        ) as imu_file:
            point_writer = csv.writer(point_file)
            imu_writer = csv.writer(imu_file)

            point_writer.writerow(["timestamp_ns", "point_id", "x", "y", "z", "intensity"])
            imu_writer.writerow(["timestamp_ns", "ax", "ay", "az", "gx", "gy", "gz"])

            while not stop_event.is_set():
                elapsed = time.monotonic() - start
                timestamp_ns = time.monotonic_ns()

                for i in range(args.samples_per_cycle):
                    phase = (point_id + i) / 20.0
                    x = 2.0 * math.cos(phase)
                    y = 2.0 * math.sin(phase)
                    z = 0.25 * math.sin(phase / 2.0)
                    intensity = int(120 + 80 * abs(math.sin(phase)))
                    point_writer.writerow([timestamp_ns, point_id + i, f"{x:.4f}", f"{y:.4f}", f"{z:.4f}", intensity])

                ax = 0.01 * math.sin(elapsed)
                ay = 0.01 * math.cos(elapsed)
                az = 9.81
                gx = 0.02 * math.sin(elapsed / 2.0)
                gy = 0.01 * math.cos(elapsed / 2.0)
                gz = 0.03
                imu_writer.writerow([timestamp_ns, f"{ax:.5f}", f"{ay:.5f}", f"{az:.5f}", f"{gx:.5f}", f"{gy:.5f}", f"{gz:.5f}"])

                point_file.flush()
                imu_file.flush()

                point_id += args.samples_per_cycle
                time.sleep(period_s)
    except Exception:
        logger.exception("LiDAR collector emulator crashed")
        return 1

    logger.info("LiDAR collector emulator stopped cleanly")
    return 0


if __name__ == "__main__":
    sys.exit(main())
