import numpy as np
import pyzed.sl as sl


zed = sl.Camera()

init = sl.InitParameters()
init.camera_resolution = sl.RESOLUTION.HD720
init.camera_fps = 30
init.depth_mode = sl.DEPTH_MODE.NEURAL
init.coordinate_units = sl.UNIT.METER
init.coordinate_system = sl.COORDINATE_SYSTEM.IMAGE
init.depth_maximum_distance = 10.0

status = zed.open(init)

if status != sl.ERROR_CODE.SUCCESS:
    print(f"Could not open ZED 2i: {status}")
    raise SystemExit(1)

runtime = sl.RuntimeParameters()
point_cloud = sl.Mat()

try:
    while True:
        # Step 1: capture and process one stereo frame
        status = zed.grab(runtime)

        if status != sl.ERROR_CODE.SUCCESS:
            print(f"grab() failed: {status}")
            continue

        # Step 2: retrieve the raw organized XYZRGBA cloud
        status = zed.retrieve_measure(
            point_cloud,
            sl.MEASURE.XYZRGBA,
            sl.MEM.CPU
        )

        if status != sl.ERROR_CODE.SUCCESS:
            print(f"retrieve_measure() failed: {status}")
            continue

        # Step 3: access it as a NumPy array
        cloud = point_cloud.get_data()

        print("Raw organized cloud retrieved")
        print("Shape:", cloud.shape)
        print("Type:", cloud.dtype)

        # Read the centre pixel
        row = cloud.shape[0] // 2
        column = cloud.shape[1] // 2

        x, y, z, packed_rgba = cloud[row, column]

        if np.isfinite([x, y, z]).all():
            print(
                f"Pixel ({column}, {row}): "
                f"X={x:.3f}, Y={y:.3f}, Z={z:.3f} metres"
            )
        else:
            print("Centre pixel has no valid depth.")

        # Step 4: save this raw frame as a PLY file
        filename = "raw_xyzrgba.ply"
        save_status = point_cloud.write(filename)

        print(f"PLY result: {save_status}")
        print(f"Saved: {filename}")
        break

finally:
    point_cloud.free()
    zed.close()