import pyrealsense2 as rs
import numpy as np
import cv2

# Configure depth, color, and IMU streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 250)
config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 200)

# Start the pipeline with configuration
pipeline.start(config)

# Create post-processing filters
decimation = rs.decimation_filter()
spatial = rs.spatial_filter()
temporal = rs.temporal_filter()
hole_filling = rs.hole_filling_filter()
threshold = rs.threshold_filter(min_dist=0.15, max_dist=10.0)

# Set filter options
spatial.set_option(rs.option.filter_magnitude, 5)
spatial.set_option(rs.option.filter_smooth_alpha, 0.6)#0.75
spatial.set_option(rs.option.filter_smooth_delta, 20)#35


temporal.set_option(rs.option.filter_smooth_alpha, 0.35)
temporal.set_option(rs.option.filter_smooth_delta, 45)

decimation.set_option(rs.option.filter_magnitude, 2)

hole_filling.set_option(rs.option.holes_fill, 1)

# Create an align object to align depth frames to color frames
align = rs.align(rs.stream.color)

# Synchronization tolerance in milliseconds
sync_tolerance = 70

try:
    while True:
        frames = pipeline.wait_for_frames()
        # aligned_frames = align.process(frames)
        #
        # depth_frame = aligned_frames.get_depth_frame()
        # color_frame = aligned_frames.get_color_frame()

        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()


        accel_frame = frames.first_or_default(rs.stream.accel)
        gyro_frame = frames.first_or_default(rs.stream.gyro)

        if not depth_frame or not color_frame or not accel_frame or not gyro_frame:
            print("not in sync")
            continue

        depth_time = depth_frame.get_timestamp()
        accel_time = accel_frame.get_timestamp()
        gyro_time = gyro_frame.get_timestamp()

        if abs(depth_time - accel_time) > sync_tolerance or abs(depth_time - gyro_time) > sync_tolerance:
            continue

        cv2.imshow('before everything', cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03))


        depth_frame = decimation.process(depth_frame)
        cv2.imshow('Decimation Applied', cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03))
        cv2.imshow('decimation Applied1',
                   cv2.applyColorMap(cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03),
                                     cv2.COLORMAP_JET))

        depth_frame = threshold.process(depth_frame)
        cv2.imshow('threshold Applied', cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03))

        cv2.imshow('Threshold Applied1',cv2.applyColorMap(cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03),cv2.COLORMAP_JET))

        # disparity_transform = rs.disparity_transform(True)
        # depth_frame = disparity_transform.process(depth_frame)


        depth_frame = spatial.process(depth_frame)
        cv2.imshow('spatial Applied', cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03))

        cv2.imshow('spatial Applied1',
                   cv2.applyColorMap(cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03),
                                     cv2.COLORMAP_JET))

        depth_frame = temporal.process(depth_frame)
        cv2.imshow('temporal Applied', cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03))

        cv2.imshow('temporal Applied1',
                   cv2.applyColorMap(cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03),
                                     cv2.COLORMAP_JET))

        # depth_to_disparity = rs.disparity_transform(False)
        # depth_frame = depth_to_disparity.process(depth_frame)

        depth_frame = hole_filling.process(depth_frame)
        cv2.imshow('holefilling Applied', cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03))

        cv2.imshow('holefilling Applied1',
                   cv2.applyColorMap(cv2.convertScaleAbs(np.asanyarray(depth_frame.get_data()), alpha=0.03),
                                     cv2.COLORMAP_JET))

        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())
        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

        # cv2.imshow('Depth Image', depth_colormap)
        # cv2.imshow('Color Image', color_image)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
finally:
    pipeline.stop()
    cv2.destroyAllWindows()