#! /usr/bin/env python
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):
    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):
    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()