import open3d as o3d
import numpy as np

def pick_points(path, keypoint_names):
    object_pcd = o3d.io.read_triangle_mesh(path)
    # pick points from two point clouds and builds correspondences
    print("1) Pick " + str(len(keypoint_names)) + " keypoint using [shift + left click]")
    print("   Press [shift + right click] to undo point picking")
    print("")
    pcd_vis = o3d.visualization.VisualizerWithVertexSelection()
    pcd_vis.create_window()
    pcd_vis.add_geometry(object_pcd)
    pcd_vis.poll_events()
    pcd_vis.update_renderer()
    #pcd_vis.run()  # user picks points
    points_count = 0
    current_keypoint_id = 0
    map_keypoint_id_to_point = {}
    print_next_keypoint_name = True
    while True:
        if print_next_keypoint_name:
            print("-------")
            print("Please select " + keypoint_names[current_keypoint_id])
            print_next_keypoint_name = False
        pcd_vis.update_renderer()
        pcd_vis.update_geometry(object_pcd)
        pcd_vis.poll_events()
        picked_points = pcd_vis.get_picked_points()
        #picked_points_coords = [point.coord for point in picked_points]
        picked_points_index = [point.index for point in picked_points]
        if len(picked_points) > points_count:
            for point in picked_points_index:
                if point not in map_keypoint_id_to_point.values():
                    map_keypoint_id_to_point[current_keypoint_id] = point
            print("point for keypoint " + keypoint_names[current_keypoint_id] + " added.")
            points_count += 1
            current_keypoint_id = max(map_keypoint_id_to_point) + 1
            if points_count >= len(keypoint_names):
                break
            print_next_keypoint_name = True
        if len(picked_points) < points_count:
            for idx in map_keypoint_id_to_point:
                if map_keypoint_id_to_point[idx] not in picked_points_index:
                    current_keypoint_id = idx
                    print("point for keypoint " + keypoint_names[current_keypoint_id] + " removed.")
            points_count += -1
            print_next_keypoint_name = True
    pcd_vis.destroy_window()
    del pcd_vis
    print(map_keypoint_id_to_point)
    picked_points_coords = []
    picked_points_idx_to_coord = {point.index: point.coord for point in picked_points}
    for i in range(len(keypoint_names)):
        picked_points_coords.append(picked_points_idx_to_coord[map_keypoint_id_to_point[i]])
    return picked_points_coords


if __name__ == "__main__":
    """
        Parse arguments from command line
    """
    import argparse
    from pathlib import Path
    import json
    parser = argparse.ArgumentParser( description='Interactive 6DoF pose annotator')
    parser.add_argument('--model-path',
                        default="/home/mikkel/data/meshes/Meshes/SchaferBoxes/Medium/schafer_box_medium.ply",
                        help='file name of the object model (.ply).')
    args = parser.parse_args()
    keypoint_names = ["glass bottom right", "glass bottom left", "glass top left", "glass top right",
                      "box front bottom right", "box front bottom left", "#box front top left", "box front top right",
                      "box back bottom right", "box back bottom left", "box back top left", "box back top right",
                      "bossard label bottom right", "bossard label bottom left", "bossard label top left",
                      "bossard label top right", "bossard sidepanel top", "bossard sidepanel bottom",
                      "bossard weight front bottom right", "bossard weight front bottom left",
                      "bossard weight back bottom left", "bossard weight back bottom right"]
    print(str(len(keypoint_names)) + " keypoints:", keypoint_names)
    picked_points = pick_points(args.model_path, keypoint_names)
    keypoints = {}
    for keypoint_name, keypoint_coord in zip(keypoint_names, picked_points):
        keypoints[keypoint_name] = np.array(keypoint_coord).tolist()
    output_dict = {
        "path_to_object_file": args.model_path,
        "keypoints": keypoints
    }
    output_path = Path(args.model_path).stem + "_keypoints.json"
    f_out = open(output_path, 'w')
    json.dump(output_dict, f_out)
    print("keypoints was saved to ", output_path)

import open3d as o3d
import numpy as np

def pick_points(path, keypoint_names):
    object_pcd = o3d.io.read_triangle_mesh(path)
    # pick points from two point clouds and builds correspondences
    print("1) Pick " + str(len(keypoint_names)) + " keypoint using [shift + left click]")
    print("   Press [shift + right click] to undo point picking")
    print("")
    pcd_vis = o3d.visualization.VisualizerWithVertexSelection()
    pcd_vis.create_window()
    pcd_vis.add_geometry(object_pcd)
    pcd_vis.poll_events()
    pcd_vis.update_renderer()
    #pcd_vis.run()  # user picks points
    points_count = 0
    current_keypoint_id = 0
    map_keypoint_id_to_point = {}
    print_next_keypoint_name = True
    while True:
        if print_next_keypoint_name:
            print("-------")
            print("Please select " + keypoint_names[current_keypoint_id])
            print_next_keypoint_name = False
        pcd_vis.update_renderer()
        pcd_vis.update_geometry(object_pcd)
        pcd_vis.poll_events()
        picked_points = pcd_vis.get_picked_points()
        #picked_points_coords = [point.coord for point in picked_points]
        picked_points_index = [point.index for point in picked_points]
        if len(picked_points) > points_count:
            for point in picked_points_index:
                if point not in map_keypoint_id_to_point.values():
                    map_keypoint_id_to_point[current_keypoint_id] = point
            print("point for keypoint " + keypoint_names[current_keypoint_id] + " added.")
            points_count += 1
            current_keypoint_id = max(map_keypoint_id_to_point) + 1
            if points_count >= len(keypoint_names):
                break
            print_next_keypoint_name = True
        if len(picked_points) < points_count:
            for idx in map_keypoint_id_to_point:
                if map_keypoint_id_to_point[idx] not in picked_points_index:
                    current_keypoint_id = idx
                    print("point for keypoint " + keypoint_names[current_keypoint_id] + " removed.")
            points_count += -1
            print_next_keypoint_name = True
    pcd_vis.destroy_window()
    del pcd_vis
    print(map_keypoint_id_to_point)
    picked_points_coords = []
    picked_points_idx_to_coord = {point.index: point.coord for point in picked_points}
    for i in range(len(keypoint_names)):
        picked_points_coords.append(picked_points_idx_to_coord[map_keypoint_id_to_point[i]])
    return picked_points_coords


if __name__ == "__main__":
    """
        Parse arguments from command line
    """
    import argparse
    from pathlib import Path
    import json
    parser = argparse.ArgumentParser( description='Interactive 6DoF pose annotator')
    parser.add_argument('--model-path',
                        default="/home/mikkel/data/meshes/Meshes/SchaferBoxes/Medium/schafer_box_medium.ply",
                        help='file name of the object model (.ply).')
    args = parser.parse_args()
    keypoint_names = ["glass bottom right", "glass bottom left", "glass top left", "glass top right",
                      "box front bottom right", "box front bottom left", "#box front top left", "box front top right",
                      "box back bottom right", "box back bottom left", "box back top left", "box back top right",
                      "bossard label bottom right", "bossard label bottom left", "bossard label top left",
                      "bossard label top right", "bossard sidepanel top", "bossard sidepanel bottom",
                      "bossard weight front bottom right", "bossard weight front bottom left",
                      "bossard weight back bottom left", "bossard weight back bottom right"]
    print(str(len(keypoint_names)) + " keypoints:", keypoint_names)
    picked_points = pick_points(args.model_path, keypoint_names)
    keypoints = {}
    for keypoint_name, keypoint_coord in zip(keypoint_names, picked_points):
        keypoints[keypoint_name] = np.array(keypoint_coord).tolist()
    output_dict = {
        "path_to_object_file": args.model_path,
        "keypoints": keypoints
    }
    output_path = Path(args.model_path).stem + "_keypoints.json"
    f_out = open(output_path, 'w')
    json.dump(output_dict, f_out)
    print("keypoints was saved to ", output_path)

# pick points from two point clouds and builds correspondences #pcd_vis.run()  # user picks points #picked_points_coords = [point.coord for point in picked_points] Parse arguments from command line