#!/usr/bin/env python3

from time import sleep
import rclpy
from rclpy.node import Node

from std_msgs.msg import String
from sensor_msgs.msg import PointCloud2
import numpy as np
from sensor_msgs_py import point_cloud2
from numpy.lib.recfunctions import structured_to_unstructured, unstructured_to_structured
from PIL import Image



class LidarSubscriber(Node):

    def __init__(self):
        super().__init__('lidar_visualizer')
        self.subscription = self.create_subscription(
            PointCloud2,
            '/lidar/points',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        # read intensity data from PointCloud2 efficiently to numpy array
        intensities = point_cloud2.read_points(msg, field_names=['intensity'], reshape_organized_cloud=True)
        intensities = structured_to_unstructured(intensities)

        print(f"The number of points hitting the car is {(intensities>254).sum()}. This is a ratio of {(intensities>254).sum()/len(intensities)}%")
        exit()

        


def main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = LidarSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()