class PickupTriangleTest():
    def __init__(self, arm=Constants.Arm.Left):
        self.arm = arm

        if self.arm == Constants.Arm.Left:
            self.toolframe = Constants.Frames.LeftTool
        else:
            self.toolframe = Constants.Frames.RightTool

        self.transFrame = Constants.Frames.Link0
        self.rotFrame = self.toolframe
            
        self.gripperControl = GripperControlClass(arm, tf.TransformListener())
        self.imageDetector = ARImageDetectionClass()
                
        self.homePose = self.imageDetector.getHomePose()

        # ar_frame is the target
        self.ar_frame = '/stereo_33'
        self.objectPose = tfx.pose([0,0,0], frame=self.ar_frame)
        tf_ar_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, self.ar_frame, wait=5)
        self.objectPose = tf_ar_to_link0 * self.objectPose
        self.objectPose = tfx.pose(self.objectPose.position, tfx.tb_angles(-90,90,0))
        self.objectPose.position.y -= .002

        rospy.sleep(3)


    def moveNearTriangle(self):
        rospy.loginfo('Moving near the triangle')

        deltaPose = Util.deltaPose(self.gripperpose, self.objectPose, self.transFrame, self.rotFrame)
        deltaPose.position.z += .05

        
        self.gripperControl.goToGripperPoseDelta(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), deltaPose)

        while not self.gripperControl.isPaused() and not rospy.is_shutdown():
            rospy.sleep(.1)

    def servoNearTriangle(self):
        rospy.loginfo('Servoing near the triangle')

        while not self.imageDetector.hasFoundGripper(self.arm) and not rospy.is_shutdown():
            rospy.loginfo('Searching for gripper')
            rospy.sleep(.1)

        rospy.loginfo('Found gripper')
        gripperPose = self.imageDetector.getGripperPose(self.arm)

        while not self.imageDetector.hasFoundObject() and not rospy.is_shutdown():
            rospy.loginfo('Searching for object')
            rospy.sleep(.1)

        rospy.loginfo('Found object')
        self.objectPose = self.imageDetector.getObjectPose() # in link 0
        self.objectPose.pose.position.z += 0.02

        transBound = .008
        rotBound = float("inf")

        maxMovement = .015

        while not Util.withinBounds(gripperPose, self.objectPose, transBound, rotBound, self.transFrame, self.rotFrame) and not rospy.is_shutdown():
            if self.gripperControl.isPaused():
                rospy.sleep(1)
                self.publishObjPose(self.objectPose)
                if self.imageDetector.hasFoundNewGripper(self.arm):
                    rospy.loginfo('paused and found new gripper')
                    gripperPose = self.imageDetector.getGripperPose(self.arm)
                    deltaPose = tfx.pose(Util.deltaPose(gripperPose, self.objectPose, Constants.Frames.Link0, self.toolframe))
                    deltaPose.position = deltaPose.position.capped(maxMovement)
                    deltaPose0Link = tfx.pose(Util.deltaPose(gripperPose, self.objectPose, Constants.Frames.Link0, Constants.Frames.Link0))
                    deltaPose0Link.position = deltaPose.position.capped(maxMovement)
                    self.publishDeltaPose(deltaPose0Link, tfx.pose(gripperPose))
                    rospy.loginfo('pres enter to move')
                    raw_input()
                    self.gripperControl.goToGripperPoseDelta(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), deltaPose, ignoreOrientation=True, duration=4)

            rospy.sleep(.1)
class ServoVsOpenVsIterTest():
    def __init__(self, arm=Constants.Arm.Left):
        self.arm = arm

        if self.arm == Constants.Arm.Left:
            self.toolframe = Constants.Frames.LeftTool
        else:
            self.toolframe = Constants.Frames.RightTool

        self.transFrame = Constants.Frames.Link0
        self.rotFrame = self.toolframe
            
        self.gripperControl = GripperControlClass(arm, tf.TransformListener())
        self.imageDetector = ARImageDetectionClass()
                
        self.homePose = self.imageDetector.getHomePose()
        
        self.gripperOpenCloseDuration = 2.5

        rospy.sleep(3)


    def goHome(self):
        self.gripperControl.goToGripperPose(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), self.homePose.pose, ignoreOrientation=True)


        while not self.gripperControl.isPaused() and not rospy.is_shutdown():
            rospy.sleep(.1)

        rospy.loginfo('Opening the gripper')
        self.gripperControl.openGripper(duration=self.gripperOpenCloseDuration)
        rospy.sleep(self.gripperOpenCloseDuration)


    def openTest(self):
        self.gripperControl.start()

        self.goHome()

        while not rospy.is_shutdown():
            if self.imageDetector.hasFoundObject:

                objectPose = self.imageDetector.getObjectPose()

                ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)

                rospy.loginfo('Press enter')
                raw_input()

                self.gripperControl.goToGripperPose(ravenGripperPose, objectPose)
                break
            
            rospy.sleep(.1)

        rospy.spin()


    def oneServoTest(self):
        self.gripperControl.start()

        self.goHome()

        while not rospy.is_shutdown():
            if self.imageDetector.hasFoundGripper(self.arm) and self.imageDetector.hasFoundObject():

                gripperPose = self.imageDetector.getGripperPose(self.arm)
                objectPose = self.imageDetector.getObjectPose()

                deltaPose = Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame)

                ravenGripperPose = self.gripperControl.getGripperPose(Constants.Frames.Link0)

                rospy.loginfo('Press enter')
                raw_input()

                self.gripperControl.goToGripperPoseDelta(ravenGripperPose, deltaPose)
                break
            
            rospy.sleep(.1)

        rospy.spin()

    def multipleServoTest(self):

        self.gripperControl.start()

        self.goHome()

        while not self.imageDetector.hasFoundGripper(self.arm) and not rospy.is_shutdown():
            rospy.loginfo('Searching for gripper')
            rospy.sleep(.1)

        rospy.loginfo('Found gripper')
        gripperPose = self.imageDetector.getGripperPose(self.arm)

        while not self.imageDetector.hasFoundObject() and not rospy.is_shutdown():
            rospy.loginfo('Searching for object')
            rospy.sleep(.1)

        rospy.loginfo('Found object')
        self.objectPose = self.imageDetector.getObjectPose()

        transBound = .008
        rotBound = float("inf")

        maxMovement = .015

        while not Util.withinBounds(gripperPose, self.objectPose, transBound, rotBound, self.transFrame, self.rotFrame) and not rospy.is_shutdown():
            if self.gripperControl.isPaused():
                rospy.sleep(.5)
                if self.imageDetector.hasFoundNewGripper(self.arm):
                    rospy.loginfo('paused and found new gripper')
                    gripperPose = self.imageDetector.getGripperPose(self.arm)
                    deltaPose = tfx.pose(Util.deltaPose(gripperPose, self.objectPose, Constants.Frames.Link0, self.toolframe))
                    deltaPose.position = deltaPose.position.capped(maxMovement)
                    
                    self.gripperControl.goToGripperPoseDelta(self.gripperControl.getGripperPose(frame=Constants.Frames.Link0), deltaPose)

            rospy.sleep(.1)