from openteach.robot.allegro.allegro import AllegroHand
from openteach.robot.allegro.allegro_retargeters import AllegroKDLControl, AllegroJointControl
import time 

class MoveJoints():
    def __init__(self):
        self.hand=AllegroHand()
    def get_joint_angles(self):
        self.angles=self.hand.get_joint_position()
        return self.angles
    def move_joint_angles(self, angles):
        self.hand.move(angles)
    def test_joint_angles(self):
        angles=self.get_joint_angles()
        angles=[-0.00913896, -0.2686364,   0.62184656 , 0.75354135,  0.01028685, -0.2634577,
                 0.58641005,  0.74047804,  0.09007385, -0.30756485,  0.7521377,   0.62535864,
                 1.2776471,   0.944332 ,   1.74945,     0.39986822]
        
        """angles=[-0.0109433,  -0.27007994 , 0.7068233  , 0.75358707 , 0.01277265, -0.26501125,
        .71085227  ,0.7403323  ,0.09685319 ,-0.2774967   ,0.7998931  , 0.7573033,
        0.28161126,  0.36030218 , 0.35111344 , 0.80508226]
        """
        
        self.move_joint_angles(angles)
        
        print(angles)

    def get_endeff_coords(self):
        joint_angles=self.get_joint_angles()
        finger_positions=self.hand.get_fingertip_coords(joint_angles)
        return finger_positions
        #self.hand.move_coords(finger_positions)

    def test_endeff_coords(self,idx):
        """endeff_coords=[ 0.03366052 , 0.05289392 , 0.10908603  ,0.03364713, -0.00034895  ,0.11188311,
  0.03723773, -0.04992783 , 0.10489956 , 0.03916171, -0.00256401, -0.00477512]"""
        endeff_coords=self.get_endeff_coords()
        endeff_coords[idx]+=0.0005
        endeff_coords[idx+1]+=0.0005
        endeff_coords[idx-1]+=0.0005
        self.hand.move_coords(endeff_coords)
        






    
if __name__ == "__main__":
    move_joints=MoveJoints()
    #move_joints.test_endeff_coords(10)
    #move_joints.test_joint_angles()
    endeff_pos=move_joints.get_endeff_coords()
    #print(endeff_pos)
    #move_joints.test_endeff_coords()
    #angles=move_joints.get_joint_angles()
    #print(angles)
