import numpy as np
from plyfile import PlyData
import sys, cv2
from tqdm import tqdm

filename1 = sys.argv[1]

if '.ply' in  filename1:
    filename1 = filename1.split('.ply')[0]
if '.png' in  filename1:
    filename1 = filename1.split('.png')[0]

print "Openning " + filename1 +'.ply'
ply1 = PlyData.read(filename1 +'.ply')
print "Openning " + filename1 +'.png'
im1 = cv2.imread(filename1 +'.png')

h, w, c = im1.shape
vertex1 = ply1['vertex']

for (i, vert) in tqdm(enumerate(vertex1)):
    if vert[0] == 0 and vert[1] == 0 and vert[2] == 0:
        im1[i/w, i%w] = [0,0,0]

def print_KP(event,x,y,flags,param):
    if event == cv2.EVENT_LBUTTONDOWN:
        if np.sum(im1[y,x]) > 0:
            cv2.circle(im1,(x,y),5,(0,0,255),1)
            print ('[X,Y]:{},{}'.format(x, y))
        else:
            print ("Keypoint {} {} do not exist in cloud".format(x, y))

cv2.namedWindow("image1")

cv2.setMouseCallback("image1", print_KP)
while(1):
    cv2.imshow("image1",im1)
    if cv2.waitKey(1) & 0xFF == 27:
        break
cv2.destroyAllWindows()