# Script to test single joint configuration

from ambf_client import Client
import csv
import pandas as pd
import numpy as np
import argparse
import time

# Connecting to AMBF client
c = Client()
c.connect()

time.sleep(1) # Wait to allow client to connect

print("Connection to client successful.")

# Setting object handles
bh = c.get_obj_handle('chassis') # bh - base handle, set to 'chassis' by default
time.sleep(2.0)
eh = c.get_obj_handle('eelink') # eh - end effector handle, set to 'ee link' by default
time.sleep(2.0)

# Check if bh connected - if simulation doesn't run, uncomment the following lines and check to make sure the joint names print
# print("Getting joint names")
# print(bh.get_joint_names())

# Getting desired joint positions from inverseKinematics
joint_pos_0_desired = 0.2
joint_pos_4_desired = 0.2
joint_pos_7_desired = 0.2
joint_pos_10_desired = 0.15
joint_pos_11_desired = 0.15

joint_pos_0_current = bh.get_joint_pos(0) # get current joint positions
joint_pos_4_current = bh.get_joint_pos(4)
joint_pos_7_current = bh.get_joint_pos(7)
joint_pos_10_current = bh.get_joint_pos(10)
joint_pos_11_current = bh.get_joint_pos(11)

e0 = joint_pos_0_desired - joint_pos_0_current # setting joint errors
e4 = joint_pos_4_desired - joint_pos_4_current
e7 = joint_pos_7_desired - joint_pos_7_current
e10 = joint_pos_10_desired - joint_pos_10_current
e11 = joint_pos_11_desired - joint_pos_11_current

epsilon_c = 0.0025 # error threshold
epsilon_r = 0.001 # roll error threshold
epsilon_t = 0.001 # tilt error threshold

while abs(e0) > epsilon_c or abs(e4) > epsilon_c or abs(e7) > epsilon_c or abs(e10) > epsilon_r or abs(e11) > epsilon_t:

    # setting desired joint positions
    bh.set_joint_pos(0, joint_pos_0_desired)
    bh.set_joint_pos(4, joint_pos_4_desired)
    bh.set_joint_pos(7, joint_pos_7_desired)
    bh.set_joint_pos(10, joint_pos_10_desired)
    bh.set_joint_pos(11, joint_pos_11_desired)

    # allow robot to move
    time.sleep(0.001)

    joint_pos_0_current = bh.get_joint_pos(0) # get current joint positions
    joint_pos_4_current = bh.get_joint_pos(4)
    joint_pos_7_current = bh.get_joint_pos(7)
    joint_pos_10_current = bh.get_joint_pos(10)
    joint_pos_11_current = bh.get_joint_pos(11)

    e0 = joint_pos_0_desired - joint_pos_0_current # setting joint errors
    e4 = joint_pos_4_desired - joint_pos_4_current
    e7 = joint_pos_7_desired - joint_pos_7_current
    e10 = joint_pos_10_desired - joint_pos_10_current
    e11 = joint_pos_11_desired - joint_pos_11_current

print("Robot converged. Current pose:")
print(eh.get_pose())