import math
from utils import RobotClient

HOST = "192.168.3.2"
PORT = 9090

config = {
    "init": {
        'pos': [0.09810244822368663, 0.3313196269764149, 0.44544069305226175], 'ori': [0.8029422807647959, -0.4820363460296128, 0.034976080376991056, -0.3488571751707414]
    },
    "pre_grasp_position": {
        'pos': [0.076875580075771, 0.4985114141223675, 0.2578368487288887], 'ori': [0.6837207910111557, -0.7294870127641278, -0.004201289362322976, -0.01889252008713971]
    },
    "grasp_position": {
        'pos': [0.04022607947465449, 0.49870956679172035, 0.12855873351195465], 'ori': [0.7108673879318799, -0.6971760905452062, 0.04979916198276968, -0.07831410481402765]
    },
    "post_grasp_position": {
        'pos': [0.04138766852425917, 0.49843996951036346, 0.17242170594713332], 'ori': [0.7105716687621472, -0.6972845331909826, 0.050898212328428484, -0.0793193249276688]
    },
    "pre_insert_position": {
        'pos': [0.15531457163779136, 0.4887644805574635, 0.30996257822689316], 'ori': [0.7096793154895238, -0.6750311908027418, 0.12151729153083345, -0.1610022001985184]
    },
    "insert_position": {
        'pos': [0.18819735499444024, 0.4933238342577449, 0.2803404963272801], 'ori': [0.7110943169204221, -0.6510710114660861, 0.06550934464000123, -0.2572157387036094]
    },
    "post_insert_position": {
        'pos': [0.20150600619054684, 0.4986867571185973, 0.24683169991220366], 'ori': [0.6922110693388366, -0.6725304109660972, 0.0757396109325311, -0.2506196184386724]
    },
    "move_away_position": {
        'pos': [0.06646318981938315, 0.28008362744706417, 0.3081809630089793], 'ori': [0.9557005454147172, 0.023550142689031436, 0.010835030563343097, -0.29319696517193]
    },
    "joint_move_away_position": {
        'pos': [-0.03705798520615846, 0.2948740118379424, 0.1380593922055155], 'ori': [0.7044778798061612, -0.7037920747711072, 0.051086947698884985, -0.07601155259569223]
    },
    "final_moveaway": {
        'pos': [0.06232913033349278, 0.28220623060555067, 0.1939445619236099], 'ori': [0.6689183667946061, -0.6855402199272477, 0.10599550652863488, -0.267110048524341]
    },
    "joint_speed_test": {
        'pos': [-0.014667855657250158, 0.21178473592030767, 0.6099448870104303], 'ori': [0.8973159098098845, -0.2554666640875886, 0.044440682660961045, -0.35719177939399194]
    },
}

def main():
    robot = RobotClient(HOST, PORT)
    robot.set_joint_maxacc(1)
    robot.set_joint_maxvelc(1)
    robot.set_end_max_line_velc(1)
    robot.set_end_max_line_acc(1)
    robot.set_end_max_angle_acc(1)
    robot.set_end_max_angle_velc(1)

    robot.move_to_pose_joint(config["final_moveaway"])
    robot.move_to_pose_cart(config["pre_grasp_position"])
    robot.move_to_pose_cart(config["grasp_position"])
    robot.move_to_pose_cart(config["post_grasp_position"])
    robot.move_to_pose_cart(config["pre_insert_position"])
    robot.move_to_pose_cart(config["insert_position"])
    robot.move_to_pose_cart(config["post_insert_position"])
    robot.move_to_pose_cart(config["final_moveaway"])

    # robot.move_to_pose_joint(config["final_moveaway"])
    # robot.move_to_pose_joint(config["joint_speed_test"])
    # robot.move_to_pose_joint(config["final_moveaway"])
    # robot.move_to_pose_joint(config["joint_speed_test"])
    # robot.move_to_pose_joint(config["final_moveaway"])
    # robot.move_to_pose_joint(config["joint_speed_test"])


    robot.close()



if __name__ == "__main__":
    main()