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

process_value_data = 0

def callbackstate(subdata_state):
    #rospy.loginfo("process value is %f", subdata_state.process_value)
    global process_value_data
    process_value_data = subdata_state.process_value


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, callbackstate)

    # back all joint to zero
    count = 0
    rate = rospy.Rate(15)  #15 hz
    while not rospy.is_shutdown():
        #t0 = rospy.Time.now().to_sec()
        angle = 0
        #angle = angle - 6.28    # better to look
        #rospy.loginfo("angle = %f , time = %f", angle, t0)
        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)

        #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data,  process_value_data)
        # print do not need global
        count = count + 1
        rospy.loginfo("count = %f ", count)
        if count == 45:
            break
        rate.sleep()
    rospy.loginfo("count 45 jump out")

    # move finger5
    count = 0
    rate_1 = rospy.Rate(15)
    while not rospy.is_shutdown():
        angle = 1.7
        move_msg.data = angle
        finger5_0_publisher.publish(move_msg)
        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("count 45 jump out")

    # 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()
    rospy.loginfo("count 45 jump out")

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

        count = count + 1
        rospy.loginfo("count = %f ", count)
        if count == 45:
            break
        rate_3.sleep()
    rospy.loginfo("count 45 jump out")

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

        count = count + 1
        rospy.loginfo("count = %f ", count)
        if count == 45:
            break
        rate_4.sleep()
    rospy.loginfo("count 45 jump out")

    # move finger1
    count = 0
    rate_5 = rospy.Rate(15)
    while not rospy.is_shutdown():
        angle = -1
        move_msg.data = angle
        finger1_rotate_publisher.publish(move_msg)
        angle = -0.1
        move_msg.data = angle
        finger1_0_publisher.publish(move_msg)
        angle = 0.5
        move_msg.data = angle
        finger1_1_publisher.publish(move_msg)

        count = count + 1
        rospy.loginfo("count = %f ", count)
        if count == 45:
            break
        rate_5.sleep()
    rospy.loginfo("count 45 jump out")

    # back all joint to zero
    count = 0
    rate_6 = rospy.Rate(15)  #15 hz
    while not rospy.is_shutdown():
        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)

        count = count + 1
        rospy.loginfo("count = %f ", count)
        if count == 45:
            break
        rate_6.sleep()
    rospy.loginfo("count 45 jump out")


    rospy.spin()

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