import numpy as np
import matplotlib.pyplot as plt
import open3d as o3d
import os
import cv2

def depth2cloud(path_rgb, path_depth, intrinsics):
    FX_DEPTH, FY_DEPTH, CX_DEPTH, CY_DEPTH = intrinsics
    # Read depth and color image:
    rgb_image = cv2.imread(path_rgb, cv2.IMREAD_UNCHANGED)
    rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2RGB)
    # depth_image = cv2.imread(path_depth, cv2.IMREAD_UNCHANGED)
    depth_image = cv2.imread(path_depth, cv2.IMREAD_ANYDEPTH).astype(np.float32) #  /100.0
    print(f"type(depth_image):{type(depth_image)}")
    print(f"type(rgb_image):{type(rgb_image)}")
    print(f"depth_image.shape:{depth_image.shape}")
    print(f"rgb_image.shape:{rgb_image.shape}")
    print(f"depth_image.dtype:{depth_image.dtype}")
    print(f"rgb_image.dtype:{rgb_image.dtype}")
    depth_image = depth_image.astype(np.int32)
    rgb_image = rgb_image.astype(np.uint8)
    color = o3d.geometry.Image(rgb_image)
    depth = o3d.geometry.Image(depth_image.astype(np.uint16))
    # Display depth and grayscale image:
    fig, axs = plt.subplots(1, 2)
    axs[0].imshow(depth, cmap="gray")
    axs[0].set_title("Depth image")
    axs[1].imshow(color)
    axs[1].set_title("RGB image")
    print(np.min(depth_image), np.max(depth_image))
    # plt.show()

    # compute point cloud:
    # Both images has the same resolution
    height, width = depth_image.shape
    pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
        width, height, FX_DEPTH, FY_DEPTH, CX_DEPTH, CY_DEPTH
    )
    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
        color, depth, convert_rgb_to_intensity=False
    )
    pcd_o3d = o3d.geometry.PointCloud.create_from_rgbd_image(
        rgbd, pinhole_camera_intrinsic
    )
    # Convert to Open3D.PointCLoud:
    pcd_o3d.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
    o3d.visualization.draw_geometries([pcd_o3d])
    o3d.io.write_point_cloud("test.ply", pcd_o3d)



if __name__ == "__main__":
    # Load the data
    path_rgb = "o1recon.png"
    path_depth = "o1spiket.png"
    path_depth = "o1ours.png"
    K = np.array([
        [725,   0, 620.5],
        [  0, 725, 187],
        [  0,   0,   1]
    ])
    intrinsics = [725, 725, 620.5, 187]
    depth2cloud(path_rgb, path_depth, intrinsics)