#!/usr/bin/env python3
"""
Minimum UV-unroll finger tactile pipeline.

Offline: load Blender export -> build_or_load_domain -> unroll_map_uv.npz
Runtime:  camera frame -> FrameUnwarper -> TactileProcessor -> forces/contact

For the full demo (UV debug panels, dashboard, benchmarks) see:
  tools/3d_patch_generation_blender/unroll_realtime.py
"""

import argparse
import os
import sys
import time

os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0")

import cv2
import orisys
from orisys.finger import FingerRuntime

from pipeline.profiling import StageProfiler, run_deformation_timed

_FINGER_PIPELINE_STAGES = (
    "capture",
    "uv_unroll",
    "flow",
    "hhd",
    "contact",
    "read_info",
    "display",
    "total",
)


def main():
    parser = argparse.ArgumentParser(description="Minimum UV-unroll finger tactile pipeline")
    parser.add_argument("--camera_id", type=int, default=0)
    parser.add_argument("--video", help="Video file path (overrides camera)")
    parser.add_argument("--config", "-c", default="./config/dd02-ov.json", help="Orisys SDK config")
    parser.add_argument("--show", action="store_true", default=True, help="Show unrolled image window")
    parser.add_argument("--verbose", action="store_true", help="SDK verbose logs")
    parser.add_argument(
        "--profile",
        action="store_true",
        help="Print per-stage timing (capture, UV, flow, HHD, contact, read_info, display)",
    )
    parser.add_argument(
        "--profile-every",
        type=int,
        default=30,
        help="When --profile is set, print rolling averages every N frames (default: 30)",
    )
    args = parser.parse_args()

    try:
        video = args.camera_id
        if args.video:
            video = args.video
        runtime = FingerRuntime.open(
            config=args.config,
            video=video,
            verbose=args.verbose,
        )
    except RuntimeError as exc:
        print(str(exc), file=sys.stderr)
        sys.exit(1)

    if args.verbose:
        geometry = runtime.geometry
        print(
            f"UV geometry: native={geometry.native_width}x{geometry.native_height}, "
            f"source={geometry.source_width}x{geometry.source_height} "
            f"output={geometry.out_width}x{geometry.out_height} "
        )

    runtime.warmup(color=False)
    tactile = runtime.tactile
    profiler = (
        StageProfiler(report_every=args.profile_every, stage_order=_FINGER_PIPELINE_STAGES)
        if args.profile
        else None
    )

    print("Press 'q' to quit, 'r' to reset tracker.")
    if args.profile:
        print(f"Profiling enabled (report every {args.profile_every} frames).")
    try:
        while True:
            frame_t0 = time.perf_counter()

            if profiler is not None:
                t0 = time.perf_counter()
                ok, frame = runtime.read_source()
                profiler.record("capture", (time.perf_counter() - t0) * 1000.0)
                if not ok:
                    break
                t0 = time.perf_counter()
                unrolled = runtime.process_frame(frame, color=False)
                profiler.record("uv_unroll", (time.perf_counter() - t0) * 1000.0)
            else:
                ok, unrolled = runtime.grab_and_process(color=False)
                if not ok:
                    break

            if profiler is not None:
                flow_ms, hhd_ms = run_deformation_timed(
                    tactile, check_motion=True, threshold=0
                )
                profiler.record("flow", flow_ms)
                profiler.record("hhd", hhd_ms)
            else:
                tactile.compute_deformation(check_motion=True, threshold=0)

            t0 = time.perf_counter()
            tactile.compute_contact()
            if profiler is not None:
                profiler.record("contact", (time.perf_counter() - t0) * 1000.0)

            t0 = time.perf_counter()
            fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = tactile.read_info(
                tactile.info.FPS,
                tactile.info.VRAW,
                tactile.info.VNORMAL,
                tactile.info.IMG,
                tactile.info.CONTOUR,
                tactile.info.CENTROID,
                tactile.info.DEPTH,
                tactile.info.FNORMAL,
                tactile.info.FSHEARX,
                tactile.info.FSHEARY,
            )
            if profiler is not None:
                profiler.record("read_info", (time.perf_counter() - t0) * 1000.0)

            status = (
                f"FPS={fps:.2f} normal={fnormal:.2f} shear=({fshearx:.2f}, {fsheary:.2f}) "
                f"mask={tactile.mask_stats['coverage']:.2f}"
            )

            if args.show:
                t0 = time.perf_counter()
                arrows = orisys.util.draw_arrows(
                    unrolled,
                    flow,
                    threshold=0.4,
                    grid_spacing=10,
                    arrow_scale=1.0,
                )
                cv2.imshow("UV Unrolled", arrows)
                if profiler is not None:
                    profiler.record("display", (time.perf_counter() - t0) * 1000.0)

            if profiler is not None:
                total_ms = (time.perf_counter() - frame_t0) * 1000.0
                profiler.record("total", total_ms)
                status += (
                    f" | wall={total_ms:.1f}ms flow={profiler.last.get('flow', 0):.1f}ms"
                    f" hhd={profiler.last.get('hhd', 0):.1f}ms"
                )
                profiler.tick()
            print(status, end="\r")

            key = cv2.waitKey(1) & 0xFF
            if key == ord("q"):
                print("\nExiting.")
                break
            if key == ord("r"):
                tactile.reset()
                print("\nTracker reset.")
    finally:
        if profiler is not None and profiler.frame_count:
            profiler.print_report(prefix="[profile final]")
        runtime.close()
        cv2.destroyAllWindows()


if __name__ == "__main__":
    main()