import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header
import sensor_msgs_py.point_cloud2 as pc2
import serial
import math
import struct

class VL53PicoPointCloud(Node):
    def __init__(self):
        super().__init__('vl53_pico_pointcloud')
        self.publisher = self.create_publisher(PointCloud2, 'vl53_pointcloud', 10)
        self.ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
        self.timer = self.create_timer(0.1, self.read_and_publish)

    def read_and_publish(self):
        line = self.ser.readline().decode().strip()
        try:
            values = list(map(float, line.split(',')))
            if len(values) != 64:
                return

            # Convert to meters and filter invalid
            distances_m = [
                d / 10.0 # eigentlich /1000 für m anstatt mm
                for d in values
            ]

            points = []
            fov_deg = 60.0
            step_angle = math.radians(fov_deg / 8)  # 8 rows & columns
            row_offset = -math.radians(fov_deg / 2) + step_angle / 2
            col_offset = -math.radians(fov_deg / 2) + step_angle / 2

            for row in range(8):
                for col in range(8):
                    idx = row * 8 + col
                    d = distances_m[idx]
                    if math.isnan(d):
                        continue

                    theta = col_offset + col * step_angle
                    phi = row_offset + row * step_angle

                    # Convert spherical to Cartesian (approximation for RViz)
                    x = d * math.cos(phi) * math.sin(theta)
                    y = d * math.sin(phi)
                    z = d * math.cos(phi) * math.cos(theta)

                    points.append((x, y, z))

            header = Header()
            header.stamp = self.get_clock().now().to_msg()
            header.frame_id = 'vl53_frame'

            fields = [
                PointField(name='x', offset=0,  datatype=PointField.FLOAT32, count=1),
                PointField(name='y', offset=4,  datatype=PointField.FLOAT32, count=1),
                PointField(name='z', offset=8,  datatype=PointField.FLOAT32, count=1)
            ]

            cloud = pc2.create_cloud(header, fields, points)
            self.publisher.publish(cloud)

        except Exception as e:
            self.get_logger().warn(f"Failed to parse or publish: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = VL53PicoPointCloud()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()