import matplotlib.pyplot as plt
import numpy as np
import pybullet as p
import time


direct = p.connect(p.GUI)#, options="--window_backend=2 --render_device=0")
#egl = p.loadPlugin("eglRendererPlugin")


p.loadURDF('plane.urdf')
p.loadURDF("r2d2.urdf",[0,0,1])
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
p.changeVisualShape(cube_trans,-1,rgbaColor=[1,1,1,0.1])
width = 128
height = 128

fov = 60
aspect = width / height
near = 0.02
far = 1

view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)

# Get depth values using the OpenGL renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True,renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl= np.reshape(images[2], (height, width, 4))*1./255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
seg_opengl = np.reshape(images[4], [width, height])*1./255.
time.sleep(1)

# Get depth values using Tiny renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_TINY_RENDERER)
depth_buffer_tiny = np.reshape(images[3], [width, height])
depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
rgb_tiny= np.reshape(images[2], (height, width, 4))*1./255.
seg_tiny = np.reshape(images[4],[width,height])*1./255.



bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
for b in range (p.getNumBodies()):
	p.changeVisualShape(b,linkIndex=-1,textureUniqueId=textureId)
	for j in range(p.getNumJoints(b)):
		p.changeVisualShape(b,linkIndex=j,textureUniqueId=textureId)

viewMat = [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0]
projMat = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
proj_opengl= np.reshape(images[2], (height, width, 4))*1./255.
time.sleep(1)

images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_TINY_RENDERER, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
proj_tiny= np.reshape(images[2], (height, width, 4))*1./255.

# Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane
plt.subplot(4, 2, 1)
plt.imshow(depth_opengl, cmap='gray', vmin=0, vmax=1)
plt.title('Depth OpenGL3')

plt.subplot(4, 2, 2)
plt.imshow(depth_tiny, cmap='gray', vmin=0, vmax=1)
plt.title('Depth TinyRenderer')

plt.subplot(4,2,3)
plt.imshow(rgb_opengl)
plt.title('RGB OpenGL3')

plt.subplot(4,2,4)
plt.imshow(rgb_tiny)
plt.title('RGB Tiny')

plt.subplot(4,2,5)
plt.imshow(seg_opengl)
plt.title('Seg OpenGL3')

plt.subplot(4,2,6)
plt.imshow(seg_tiny)
plt.title('Seg Tiny')

plt.subplot(4,2,7)
plt.imshow(proj_opengl)
plt.title('Proj OpenGL')

plt.subplot(4,2,8)
plt.imshow(proj_tiny)
plt.title('Proj Tiny')
plt.subplots_adjust(hspace=0.7)


plt.show()