#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64
from control_msgs.msg import JointControllerState
import math  # for sin

finger5_0_process_value = 0
finger5_1_process_value = 0
finger4_0_process_value = 0
finger4_1_process_value = 0
finger3_0_process_value = 0
finger3_1_process_value = 0
finger2_0_process_value = 0
finger2_1_process_value = 0
finger1_rotate_axis_process_value = 0
finger1_0_base_process_value = 0
finger1_1_process_value = 0

def finger5_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger5_0_process_value
    finger5_0_process_value = subdata.process_value

def finger5_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger5_1_process_value
    finger5_1_process_value = subdata.process_value

def finger4_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger4_0_process_value
    finger4_0_process_value = subdata.process_value

def finger4_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger4_1_process_value
    finger4_1_process_value = subdata.process_value

def finger3_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger3_0_process_value
    finger3_0_process_value = subdata.process_value

def finger3_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger3_1_process_value
    finger3_1_process_value = subdata.process_value

def finger2_0_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger2_0_process_value
    finger2_0_process_value = subdata.process_value

def finger2_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger2_1_process_value
    finger2_1_process_value = subdata.process_value

def finger1_rotate_axis_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger1_rotate_axis_process_value
    finger1_rotate_axis_process_value = subdata.process_value

def finger1_0_base_callback(subdata):
    #rospy.loginfo("process value1 is %f", subdata.process_value)
    global finger1_0_base_process_value
    finger1_0_base_process_value = subdata.process_value

def finger1_1_callback(subdata):
    #rospy.loginfo("process value2 is %f", subdata.process_value)
    global finger1_1_process_value
    finger1_1_process_value = subdata.process_value


#def hand_initial():
    # back every joint angle to zero

def check_is_arrive(finger_arg, expect_value, expect_limit):
    # check finger's position
    difference = finger_arg - expect_value
    origin_difference = difference

    # notice pi's number!!!!!!!!!!!
    while difference < 0 or difference >= 6.28318530718:
        if difference < 0:
            difference = difference + 6.28318530718
        if difference >= 6.28318530718:
            difference = difference - 6.28318530718

    bound_difference = difference
    final_difference = bound_difference
    if difference > 3.14159265359:
        final_difference = 6.28318530718 - difference

    rospy.loginfo("process value is %f, expect is %f, - = %f, 0 ~ 6.28 = %f, final = %f", finger_arg, expect_value, origin_difference, bound_difference, final_difference)

    if final_difference < expect_limit:
        return True
    else:
        return False

def movehand():
    rospy.init_node("my_hand_command", anonymous=True)
    finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20)
    finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20)
    finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20)
    finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20)
    finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20)
    finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20)
    finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20)
    finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20)
    finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20)
    finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20)
    finger1_1_publisher = rospy.Publisher("/finger1_1_controller/command", Float64, queue_size=20)
    move_msg = Float64()

    rospy.Subscriber('/finger5_0_controller/state', JointControllerState, finger5_0_callback)
    rospy.Subscriber('/finger5_1_controller/state', JointControllerState, finger5_1_callback)
    rospy.Subscriber('/finger4_0_controller/state', JointControllerState, finger4_0_callback)
    rospy.Subscriber('/finger4_1_controller/state', JointControllerState, finger4_1_callback)
    rospy.Subscriber('/finger3_0_controller/state', JointControllerState, finger3_0_callback)
    rospy.Subscriber('/finger3_1_controller/state', JointControllerState, finger3_1_callback)
    rospy.Subscriber('/finger2_0_controller/state', JointControllerState, finger2_0_callback)
    rospy.Subscriber('/finger2_1_controller/state', JointControllerState, finger2_1_callback)

    rospy.Subscriber('/finger1_rotate_axis_controller/state', JointControllerState, finger1_rotate_axis_callback)
    rospy.Subscriber('/finger1_0_base_controller/state', JointControllerState, finger1_0_base_callback)
    rospy.Subscriber('/finger1_1_controller/state', JointControllerState, finger1_1_callback)


    # all yes_count initial
    yes_count1 = 0
    yes_count2 = 0
    yes_count3 = 0
    yes_count4 = 0
    yes_count5 = 0
    yes_count6 = 0
    yes_count7 = 0
    yes_count8 = 0
    yes_count9 = 0
    yes_count10 = 0
    yes_count11 = 0

    # all is_break
    is_break1 = False
    is_break2 = False
    is_break3 = False
    is_break4 = False
    is_break5 = False
    is_break6 = False
    is_break7 = False
    is_break8 = False
    is_break9 = False
    is_break10 = False
    is_break11 = False

    # back all joint to zero
    count = 0
    rate = rospy.Rate(15)  #15 hz
    while not rospy.is_shutdown():
        #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data,  process_value_data)
        # print do not need global

        angle = 0
        move_msg.data = angle

        finger5_0_publisher.publish(move_msg)
        finger5_1_publisher.publish(move_msg)
        finger4_0_publisher.publish(move_msg)
        finger4_1_publisher.publish(move_msg)
        finger3_0_publisher.publish(move_msg)
        finger3_1_publisher.publish(move_msg)
        finger2_0_publisher.publish(move_msg)
        finger2_1_publisher.publish(move_msg)
        finger1_rotate_publisher.publish(move_msg)
        finger1_0_publisher.publish(move_msg)
        finger1_1_publisher.publish(move_msg)

        if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and
            check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and
            check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and
            check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and
            check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and
            check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)):
            break

        rate.sleep()
    rospy.loginfo("finish open the hand !!!!!!")



    # from finger5 start change, use process_value
    # move finger5
    count = 0
    rate_1 = rospy.Rate(15)
    while not rospy.is_shutdown():
        angle = 1.7
        move_msg.data = angle

        if not check_is_arrive(finger5_0_process_value, angle, 0.01):
            finger5_0_publisher.publish(move_msg)
        else:
            yes_count1 = yes_count1 + 1
            rospy.loginfo("yes_count1 = %f", yes_count1)

        if yes_count1 == 5:
            # break
            is_break1 = True

        angle = 1
        move_msg.data = angle

        if not check_is_arrive(finger5_1_process_value, angle, 0.1):
            finger5_1_publisher.publish(move_msg)
        else:
            yes_count2 = yes_count2 + 1
            rospy.loginfo("yes_count2 = %f", yes_count2)

        if yes_count2 == 15:
            # break
            is_break2 = True

        if is_break1 and is_break2:
            yes_count1 = 0
            yes_count2 = 0
            is_break1 = False
            is_break2 = False
            break
#        angle = 1
#        move_msg.data = angle
#        finger5_1_publisher.publish(move_msg)

#        count = count + 1
#        rospy.loginfo("count = %f ", count)
#        if count == 45:
#            break
        rate_1.sleep()
    rospy.loginfo("finger5 finish !!!!!!!!!!")

    # move finger4
    count = 0
    rate_2 = rospy.Rate(15)
    while not rospy.is_shutdown():
#        angle = 1.7
#        move_msg.data = angle
#        finger4_0_publisher.publish(move_msg)
#        angle = 1
#        move_msg.data = angle
#        finger4_1_publisher.publish(move_msg)

#        count = count + 1
#        rospy.loginfo("count = %f ", count)
#        if count == 45:
#            break
#        rate_2.sleep()
        angle = 1.7
        move_msg.data = angle

        if not check_is_arrive(finger4_0_process_value, angle, 0.01):
            finger4_0_publisher.publish(move_msg)
        else:
            yes_count1 = yes_count1 + 1
            rospy.loginfo("yes_count1 = %f", yes_count1)

        if yes_count1 == 5:
            # break
            is_break1 = True

        angle = 1
        move_msg.data = angle

        if not check_is_arrive(finger4_1_process_value, angle, 0.1):
            finger4_1_publisher.publish(move_msg)
        else:
            yes_count2 = yes_count2 + 1
            rospy.loginfo("yes_count2 = %f", yes_count2)

        if yes_count2 == 15:
            # break
            is_break2 = True

        if is_break1 and is_break2:
            yes_count1 = 0
            yes_count2 = 0
            is_break1 = False
            is_break2 = False
            break

        rate_2.sleep()

    rospy.loginfo("finger4 finish !!!!!!!!!!")

    # move finger3
    count = 0
    rate_3 = rospy.Rate(15)
    while not rospy.is_shutdown():
        angle = 1.7
        move_msg.data = angle

        if not check_is_arrive(finger3_0_process_value, angle, 0.01):
            finger3_0_publisher.publish(move_msg)
        else:
            yes_count1 = yes_count1 + 1
            rospy.loginfo("yes_count1 = %f", yes_count1)

        if yes_count1 == 5:
            # break
            is_break1 = True

        angle = 1
        move_msg.data = angle

        if not check_is_arrive(finger3_1_process_value, angle, 0.1):
            finger3_1_publisher.publish(move_msg)
        else:
            yes_count2 = yes_count2 + 1
            rospy.loginfo("yes_count2 = %f", yes_count2)

        if yes_count2 == 15:
            # break
            is_break2 = True

        if is_break1 and is_break2:
            yes_count1 = 0
            yes_count2 = 0
            is_break1 = False
            is_break2 = False
            break

        rate_3.sleep()

    rospy.loginfo("finger3 finish !!!!!!!!!!")

    # move finger2
    count = 0
    rate_4 = rospy.Rate(15)
    while not rospy.is_shutdown():
        angle = 1.7
        move_msg.data = angle

        if not check_is_arrive(finger2_0_process_value, angle, 0.01):
            finger2_0_publisher.publish(move_msg)
        else:
            yes_count1 = yes_count1 + 1
            rospy.loginfo("yes_count1 = %f", yes_count1)

        if yes_count1 == 5:
            # break
            is_break1 = True

        angle = 1
        move_msg.data = angle

        if not check_is_arrive(finger2_1_process_value, angle, 0.1):
            finger2_1_publisher.publish(move_msg)
        else:
            yes_count2 = yes_count2 + 1
            rospy.loginfo("yes_count2 = %f", yes_count2)

        if yes_count2 == 15:
            # break
            is_break2 = True

        if is_break1 and is_break2:
            yes_count1 = 0
            yes_count2 = 0
            is_break1 = False
            is_break2 = False
            break

        rate_4.sleep()

    rospy.loginfo("finger2 finish !!!!!!!!!!")


    # move finger1
    count = 0
    rate_5 = rospy.Rate(15)
    while not rospy.is_shutdown():

        angle = 1
        # may need change -- in .xacro
        move_msg.data = angle

        if not check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01):
            finger1_rotate_publisher.publish(move_msg)
        else:
            yes_count1 = yes_count1 + 1
            rospy.loginfo("yes_count1 = %f", yes_count1)

        if yes_count1 == 15:
            # break
            is_break1 = True

        angle = -0.1
        move_msg.data = angle

        if not check_is_arrive(finger1_0_base_process_value, angle, 0.01):
            finger1_0_publisher.publish(move_msg)
        else:
            yes_count2 = yes_count2 + 1
            rospy.loginfo("yes_count2 = %f", yes_count2)

        if yes_count2 == 15:
            # break
            is_break2 = True

        angle = 0.5
        move_msg.data = angle

        if not check_is_arrive(finger1_1_process_value, angle, 0.01):
            finger1_1_publisher.publish(move_msg)
        else:
            yes_count3 = yes_count3 + 1
            rospy.loginfo("yes_count3 = %f", yes_count3)

        if yes_count3 == 15:
            # break
            is_break3 = True


        if is_break1 and is_break2 and is_break3:
            yes_count1 = 0
            yes_count2 = 0
            yes_count3 = 0
            is_break1 = False
            is_break2 = False
            is_break3 = False
            break

        rate_5.sleep()

    rospy.loginfo("finger1 finish !!!!!!!!!!")


    # back all joint to zero
    count = 0
    rate_6 = rospy.Rate(15)  #15 hz
    while not rospy.is_shutdown():
        #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data,  process_value_data)
        # print do not need global

        angle = 0
        move_msg.data = angle

        finger5_0_publisher.publish(move_msg)
        finger5_1_publisher.publish(move_msg)
        finger4_0_publisher.publish(move_msg)
        finger4_1_publisher.publish(move_msg)
        finger3_0_publisher.publish(move_msg)
        finger3_1_publisher.publish(move_msg)
        finger2_0_publisher.publish(move_msg)
        finger2_1_publisher.publish(move_msg)
        finger1_rotate_publisher.publish(move_msg)
        finger1_0_publisher.publish(move_msg)
        finger1_1_publisher.publish(move_msg)

        if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and
            check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and
            check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and
            check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and
            check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and
            check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)):
            break

        rate_6.sleep()
    rospy.loginfo("finish open the hand !!!!!!")



    rospy.spin()

if __name__ == '__main__':
    try:
        movehand()
    except rospy.ROSInterruptException: pass