#! /usr/bin/env python
"""A small module for hand controls service.

ROS Node: `hand_ler`
Provides `hands` service.
"""
import os

import rospy
from naoqi import ALProxy
from threading import Thread

from teleoperation.srv import Hands, HandsResponse


mp = None


def do_in_parallel(func):
    """Open hands in parallel.

    The robot's hand function blocks and only accepts one hand. This is a
    thread-based hackaround.

    """
    tl = Thread(target=func, args=('LHand',))
    tr = Thread(target=func, args=('RHand',))
    tl.start()
    tr.start()
    while tr.is_alive() and tl.is_alive():
        rospy.Rate(10).sleep()


def handle_hands(request):
    """Hand service routine."""
    if request.target == 'open':
        do_in_parallel(mp.openHand)
        return HandsResponse('opened')
    elif request.target == 'close':
        do_in_parallel(mp.closeHand)
        return HandsResponse('closed')
    else:
        return HandsResponse('nope')


if __name__ == '__main__':
    rospy.init_node('hand_ler')
    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
    mp.wakeUp()
    hands = rospy.Service('hands', Hands, handle_hands)
    rospy.spin()
    mp.rest()
