from utils.data_load import DataLoadUtil
from utils.control_util import ControlUtil
from utils.view_util import ViewUtil
from utils.pts_util import PtsUtil
from utils.communicate_util import CommunicateUtil
import numpy as np
import os
if __name__  == "__main__":
    idx = "2"
    ControlUtil.connect_robot()
    ControlUtil.franka_reset()
    root_path = "/home/yan20/nbv_rec/project/franka_control/temp_output/cad_model_world"
    frame_path = os.path.join(root_path, idx)
    cam_info = DataLoadUtil.load_cam_info(frame_path, binocular=True)
    render_camL_to_world = cam_info["cam_to_world"]
    render_camO_to_world = cam_info["cam_to_world_O"]
    L_to_O = np.dot(np.linalg.inv(render_camO_to_world), render_camL_to_world)
    
    ControlUtil.set_pose(render_camO_to_world)

    real_camO_to_world = ControlUtil.get_pose()
    real_camL_to_world = np.dot(real_camO_to_world,L_to_O)
    view_data = CommunicateUtil.get_view_data(init=True)
    cam_pts = ViewUtil.get_pts(view_data)
    np.savetxt(f"cam_pts_{idx}.txt", cam_pts)
    world_pts = PtsUtil.transform_point_cloud(cam_pts, render_camL_to_world)
    print(real_camL_to_world)
    np.savetxt(f"world_pts_{idx}.txt", world_pts)
    # import ipdb;ipdb.set_trace()
    
    # view_data = CommunicateUtil.get_view_data()
    # cam_pts = ViewUtil.get_pts(view_data)
    # np.savetxt(f"cam_pts_{idx}.txt", cam_pts)