#! /usr/bin/env python
import os

import rospy
from naoqi import ALProxy, ALBroker, ALModule

from masterloop import inform_masterloop_factory


fall_broker = None
almem = None


_inform_masterloop = inform_masterloop_factory('fall_detector')


class FallDetectorModule(ALModule):

    def __init__(self, name):
        ALModule.__init__(self, name)
        self.mp = ALProxy('ALMotion')

    def on_robot_falling(self, *_args):
        _inform_masterloop('falling')

    def on_robot_fallen(self, *_args):
        if not _inform_masterloop('fallen'):
            return
        self.mp.rest()
        rospy.Rate(0.5).sleep()
        self.mp.wakeUp()
        pp = ALProxy('ALRobotPosture')
        while not pp.goToPosture('StandInit', 1.0):
            pass
        rospy.Rate(1).sleep()
        _inform_masterloop('recovered')


if __name__ == "__main__":
    rospy.init_node('fall_detector')
    rospy.wait_for_service('inform_masterloop')
    fall_broker = ALBroker("fall_broker", "0.0.0.0", 0,
                           os.environ['NAO_IP'], 9559)
    fall_detector = FallDetectorModule("fall_detector")
    almem = ALProxy("ALMemory")
    almem.subscribeToEvent("robotIsFalling",
                           "fall_detector",
                           "on_robot_falling")
    almem.subscribeToEvent("robotHasFallen",
                           "fall_detector",
                           "on_robot_fallen")

    while not rospy.is_shutdown():
        rospy.Rate(10).sleep()

    fall_broker.shutdown()