#!/usr/bin/env python3
"""Standalone test for SimVideoStreamer: generates a color-cycling SBS test pattern
and streams it to a TCP server (e.g. PICO VR headset).

Usage:
    # 1. On PICO: select SIM-Stereo, click Listen
    # 2. On PC:
    python scripts/test_sim_streamer.py --host 172.20.103.171 --port 12345

    The PICO should display a side-by-side test pattern:
    left half = cycling red, right half = cycling blue.
"""

import argparse
import time

import numpy as np


def main():
    parser = argparse.ArgumentParser(description="Test SimVideoStreamer with synthetic frames")
    parser.add_argument("--host", type=str, required=True, help="PICO IP address (TCP server)")
    parser.add_argument("--port", type=int, default=12345, help="TCP port")
    parser.add_argument("--width", type=int, default=960, help="Eye width")
    parser.add_argument("--height", type=int, default=540, help="Eye height")
    parser.add_argument("--fps", type=int, default=30, help="Target FPS")
    parser.add_argument("--duration", type=int, default=30, help="Stream duration in seconds")
    args = parser.parse_args()

    # Direct import to avoid pulling in the full mindbot/isaaclab dependency chain
    import importlib.util, pathlib
    _spec = importlib.util.spec_from_file_location(
        "sim_video_streamer",
        pathlib.Path(__file__).resolve().parents[1] / "source/mindbot/mindbot/utils/sim_video_streamer.py",
    )
    _mod = importlib.util.module_from_spec(_spec)
    _spec.loader.exec_module(_mod)
    SimVideoStreamer = _mod.SimVideoStreamer

    streamer = SimVideoStreamer(
        host=args.host,
        port=args.port,
        width=args.width,
        height=args.height,
        fps=args.fps,
    )

    print(f"Connecting to {args.host}:{args.port}...")
    if not streamer.connect():
        print("Failed to connect. Is PICO listening?")
        return

    print(f"Streaming {args.width*2}x{args.height} SBS test pattern for {args.duration}s...")
    frame_interval = 1.0 / args.fps
    start_time = time.time()
    frame_count = 0

    try:
        while time.time() - start_time < args.duration:
            t = time.time() - start_time

            # Generate test pattern: cycling colors
            # Left eye: red gradient with horizontal bars
            left = np.zeros((args.height, args.width, 3), dtype=np.uint8)
            red_val = int(128 + 127 * np.sin(t * 2.0))
            left[:, :, 0] = red_val  # R channel
            # Add horizontal position indicator
            bar_y = int((args.height - 20) * (0.5 + 0.5 * np.sin(t * 1.5)))
            left[bar_y:bar_y+20, :, :] = 255

            # Right eye: blue gradient with horizontal bars
            right = np.zeros((args.height, args.width, 3), dtype=np.uint8)
            blue_val = int(128 + 127 * np.sin(t * 2.0 + 1.0))
            right[:, :, 2] = blue_val  # B channel
            right[bar_y:bar_y+20, :, :] = 255  # same bar position for stereo reference

            if not streamer.send_frame(left, right):
                print("Connection lost.")
                break

            frame_count += 1
            # Maintain frame rate
            elapsed = time.time() - start_time
            expected = frame_count * frame_interval
            if expected > elapsed:
                time.sleep(expected - elapsed)

    except KeyboardInterrupt:
        print("\nStopped by user.")

    elapsed = time.time() - start_time
    print(f"Sent {frame_count} frames in {elapsed:.1f}s ({frame_count/elapsed:.1f} fps)")
    streamer.close()


if __name__ == "__main__":
    main()