import pyrealsense2 as rs
import numpy as np
import cv2


# mouse callback function to display distance on double click
def get_distance(event,x,y,flags,param):
    if event == cv2.EVENT_LBUTTONDBLCLK:

        depth = frames.get_depth_frame()
        color = frames.get_color_frame()
        pixel_distance_in_meters = depth.get_distance(int(x),int(y))

        print(pixel_distance_in_meters)




    return;


# Configure depth and color streams
pipeline = rs.pipeline()

config = rs.config()


# Start streaming
profile = pipeline.start(config)

ctx = rs.context()

if len(ctx.devices) > 0:
    for d in ctx.devices:
        print ('Found device: ',
        d.get_info(rs.camera_info.name), ' ',
        d.get_info(rs.camera_info.serial_number))
else:
        print("No Intel Device connected")

try:
    while True:

        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        points = rs.points
        pc = rs.pointcloud
        pc.map_to(color_frame)
        points = pc.calculate(depth_frame)
        print("Saving to 1.ply...")
        points.export_to_ply("1.ply", color_frame)
        print("Done")


        if not depth_frame or not color_frame:
            continue

        # Convert images to numpy arrays
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())


        # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

        # Stack both images horizontally
        images = depth_colormap

        # Show images
        cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
        cv2.setMouseCallback('RealSense',get_distance)
        cv2.imshow('RealSense', images)
        cv2.waitKey(1)

finally:

    # Stop streaming
    pipeline.stop()