"""WWW-POSIM WAM-V ROS sensor plotting example · IOES-Lab · MIT.

Circles enclose compact measured returns; they are not fitted buoy sizes.
Run with the installed app's prepared ROS CLI in a running WAM-V workspace.
"""
import argparse
import io
import json
import math
from pathlib import Path
import time


def stamp(message):
    value = message.header.stamp
    return value.sec + value.nanosec * 1e-9


def sensor_topics(graph, scan_topic=None, camera_topic=None):
    """Discover sensors by message type, without assuming a robot namespace."""
    selected = {}
    for kind, message_type, requested in (
            ('scan', 'sensor_msgs/msg/LaserScan', scan_topic),
            ('camera', 'sensor_msgs/msg/CompressedImage', camera_topic)):
        matches = sorted({name for name, types in graph if message_type in types
            and (requested or not any(part.startswith('_') for part in name.split('/')))})
        if requested:
            if requested in matches:
                selected[kind] = requested
            elif any(name == requested for name, _ in graph):
                raise RuntimeError(f'{requested} does not publish {message_type}. Check ros2 topic list -t.')
        elif len(matches) == 1:
            selected[kind] = matches[0]
        elif len(matches) > 1:
            raise RuntimeError(f'Multiple {kind} topics: {", ".join(matches)}. Choose one with --{kind}-topic.')
    return selected


def obstacle_circles(scan):
    points, groups, group = [], [], []
    for index, distance in enumerate(scan.ranges):
        if not math.isfinite(distance) or not scan.range_min <= distance < scan.range_max:
            if group:
                groups.append(group)
                group = []
            continue
        angle = scan.angle_min + index * scan.angle_increment
        point = (distance * math.cos(angle), distance * math.sin(angle))
        points.append(point)
        if distance >= 35.0:
            if group:
                groups.append(group)
                group = []
            continue
        if group and math.dist(group[-1], point) > max(0.3, 0.03 * distance):
            groups.append(group)
            group = []
        group.append(point)
    if group:
        groups.append(group)
    if (len(groups) > 1 and abs(scan.angle_increment) * len(scan.ranges) > 6.1
            and math.dist(groups[0][0], groups[-1][-1]) < 0.3):
        groups[0] = groups.pop() + groups[0]
    circles = []
    for group in groups:
        if len(group) < 3:
            continue
        centre = tuple(sum(point[k] for point in group) / len(group) for k in (0, 1))
        radius = max(0.2, max(math.dist(centre, point) for point in group) + 0.1)
        if radius <= 2.0:
            circles.append(dict(x_m=centre[0], y_m=centre[1], radius_m=radius, points=len(group)))
    return points, circles


def save(scan, camera, counts, output, topics=None):
    import matplotlib
    matplotlib.use('Agg')
    import matplotlib.pyplot as plt
    from matplotlib.patches import Circle
    from PIL import Image

    points, circles = obstacle_circles(scan)
    extent = (scan.range_max if math.isfinite(scan.range_max) and scan.range_max > 0
              else max((math.hypot(*point) for point in points), default=35.0))
    jpeg = bytes(camera.data)
    image = Image.open(io.BytesIO(jpeg)).convert('RGB')
    fig, axes = plt.subplots(1, 2, figsize=(13, 6), layout='constrained')
    axes[0].imshow(image)
    axes[0].axis('off')
    axes[0].set_title(f'Camera · simulation {stamp(camera):.2f}s')
    if points:
        axes[1].scatter([p[1] for p in points], [p[0] for p in points], s=6, color='#008b8b')
    for number, circle in enumerate(circles, 1):
        centre = (circle['y_m'], circle['x_m'])
        axes[1].add_patch(Circle(centre, circle['radius_m'], fill=False, color='#ff593b', linewidth=2))
        axes[1].annotate(str(number), centre)
    axes[1].scatter([0], [0], marker='^', color='black', s=100, label='LiDAR origin')
    axes[1].set(xlabel='Left y (m)', ylabel='Forward x (m)', xlim=(-extent, extent), ylim=(-extent, extent), aspect='equal')
    axes[1].invert_xaxis()
    axes[1].grid(alpha=0.3)
    axes[1].legend()
    axes[1].set_title(f'Observed-return circles · simulation {stamp(scan):.2f}s')
    delta = abs(stamp(scan) - stamp(camera))
    fig.suptitle(f'WAM-V · {len(circles)} candidate groups · sensor time difference {delta:.2f}s')
    output.mkdir(parents=True, exist_ok=True)
    fig.savefig(output / 'wamv-lidar-camera.png', dpi=160)
    plt.close(fig)
    (output / 'wamv-camera.jpg').write_bytes(jpeg)
    metadata = dict(received_messages=counts, topics=topics or {}, scan_frame=scan.header.frame_id,
        camera_frame=camera.header.frame_id, scan_time=stamp(scan), camera_time=stamp(camera),
        time_difference_seconds=delta, angle_min=scan.angle_min, angle_increment=scan.angle_increment,
        range_min=scan.range_min, range_max=scan.range_max, plot_range_m=extent,
        ranges_m=[r if math.isfinite(r) else None for r in scan.ranges], circles=circles,
        circle_meaning='Measured-return enclosure with 0.1 m padding; not physical buoy radius')
    (output / 'wamv-sensor-data.json').write_text(json.dumps(metadata, indent=2, allow_nan=False), encoding='utf-8')
    print(json.dumps(dict(output=str(output.resolve()), received_messages=counts, circles=len(circles),
                         time_difference_seconds=delta)), flush=True)


def main():
    import rclpy
    from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
    from sensor_msgs.msg import LaserScan, CompressedImage

    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--output', type=Path, default=Path('wamv-results'))
    parser.add_argument('--scan-topic', help='LaserScan topic; otherwise discover the single available topic')
    parser.add_argument('--camera-topic', help='CompressedImage topic; otherwise discover the single available topic')
    args = parser.parse_args()
    rclpy.init()
    node = rclpy.create_node('wamv_sensor_plot')
    latest, receipt, counts = {}, {}, dict(scan=0, camera=0)
    def receive(kind, message):
        latest[kind], receipt[kind] = message, time.monotonic()
        counts[kind] += 1
    qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.VOLATILE)
    try:
        deadline = time.monotonic() + 10
        while rclpy.ok():
            topics = sensor_topics(node.get_topic_names_and_types(), args.scan_topic, args.camera_topic)
            if len(topics) == 2:
                break
            if time.monotonic() >= deadline:
                missing = ', '.join(kind for kind in ('scan', 'camera') if kind not in topics)
                raise RuntimeError(f'Missing sensor topics: {missing}. Select Forward camera and 2D lidar in Workspace, '
                    'start the WAM-V world, and prepare the ROS environment again. Check ros2 topic list -t '
                    'and any --scan-topic / --camera-topic overrides.')
            rclpy.spin_once(node, timeout_sec=0.1)
        if not rclpy.ok():
            raise RuntimeError('ROS stopped before sensor discovery.')
        print(json.dumps(dict(sensor_topics=topics)), flush=True)
        subscriptions = [node.create_subscription(LaserScan, topics['scan'], lambda m: receive('scan', m), qos),
            node.create_subscription(CompressedImage, topics['camera'], lambda m: receive('camera', m), qos)]
        deadline = time.monotonic() + 60
        while rclpy.ok():
            rclpy.spin_once(node, timeout_sec=0.1)
            if (len(latest) == 2 and all(time.monotonic() - t < 3 for t in receipt.values())
                    and abs(stamp(latest['scan']) - stamp(latest['camera'])) <= 1.5):
                save(latest['scan'], latest['camera'], counts, args.output, topics)
                return
            if time.monotonic() >= deadline:
                raise RuntimeError(f'No fresh matching LiDAR/camera pair within 60 s: {counts}. Check the running WAM-V world and ROS relay.')
        raise RuntimeError('ROS stopped before sensor capture.')
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()
