open3d

import open3d as o3d
import open3d.core as o3c
import numpy as np
import matplotlib.pyplot as plt
import copy
import os
import sys

import pybullet as p
import numpy as np
import pybullet_data
import time

# Initialize the physics simulation
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# Load the objects into the simulation
planeId = p.loadURDF("plane.urdf")
#sphereId = p.loadURDF("sphere_small.urdf", [0, 1, 0])
#cylinderId = p.loadURDF("sphere_small.urdf", [0.5, 1, 0])
#robotId = p.loadURDF("r2d2.urdf", [0.5,0.5,0.5],useFixedBase=True)

while True:
    # Get the image from the camera
    camera_pos = [0, 1, 2]
    viewMatrix = p.computeViewMatrix(
        cameraEyePosition=camera_pos,
        cameraTargetPosition=[0, 1, 0],
        cameraUpVector=[0, 1, 0])

    # Set the intrinsic parameters of the camera
    fov = 60
    aspect = 320.0 / 240.0
    near = 0.01
    far = 100

    # Get the image from the camera
    width, height, rgbImg, depthImg, segImg = p.getCameraImage(width=320, height=240, viewMatrix=viewMatrix,
                                                               projectionMatrix=p.computeProjectionMatrixFOV(fov,
                                                                                                             aspect,
                                                                                                             near, far))
    # Convert the depth image to a point cloud
    points = np.zeros((height * width, 3))
    for y in range(height):
        for x in range(width):
            depth = depthImg[y, x]
            if depth != 0:
                points[y * width + x, 0] = (x - 160) * depth / 160.0
                points[y * width + x, 1] = (y - 120) * depth / 120.0
                points[y * width + x, 2] = depth
    print(points)

    print("Load a ply point cloud, print it, and render it")
    ply_point_cloud = o3d.data.PLYPointCloud()
    #pcd = o3d.t.io.read_point_cloud(ply_point_cloud.path)
    # Create a point cloud from python list.
    pcd = o3d.t.geometry.PointCloud(points)
    print(pcd, "\n")

    """o3d.visualization.draw_geometries([pcd.to_legacy()],
                                      zoom=0.3412,
                                      front=[0.4257, -0.2125, -0.8795],
                                      lookat=[2.6172, 2.0475, 1.532],
                                      up=[-0.0694, -0.9768, 0.2024])"""
    #normals = pcd.point.normals
    print("Print first 5 normals of the downsampled point cloud.")
    #print(normals[:5], "\n")
    print("Convert normals tensor into numpy array.")
    #normals_np = normals.numpy()
    #print(normals_np[:5])
    print("finish")