#! /usr/bin/env python
import time
import os

import rospy
from naoqi import ALProxy
from naoqi import ALBroker
from naoqi import ALModule

from teleoperation.srv import InformController


fall_broker = None
almem = None


def _inform_controller(what):
    inform_controller = rospy.ServiceProxy('inform_controller',
                                           InformController)
    return inform_controller('fall_detector', what).permission


class FallDetectorModule(ALModule):

    def __init__(self, name):
        ALModule.__init__(self, name)

    def on_robot_falling(self, *_args):
        print('falling')
        _inform_controller('falling')

    def on_robot_fallen(self, *_args):
        print('fallen')
        _inform_controller('fallen')


if __name__ == "__main__":
    rospy.init_node('fall_detector')
    rospy.wait_for_service('inform_controller')
    print 'create broker'
    fall_broker = ALBroker("fall_broker", "0.0.0.0", 0,
                           os.environ['NAO_IP'], 9559)
    print 'create instance'
    fall_detector = FallDetectorModule("fall_detector")
    almem = ALProxy("ALMemory")
    print 'subscribe'
    almem.subscribeToEvent("robotIsFalling",
                           "fall_detector",
                           "on_robot_falling")
    almem.subscribeToEvent("robotHasFallen",
                           "fall_detector",
                           "on_robot_fallen")
    print 'init success'
    while not rospy.is_shutdown():
        time.sleep(1)
    print
    print "Interrupted by user, shutting down"
    fall_detector.broker.shutdown()