#! /usr/bin/env python
import rospy

from teleoperation.srv import InformController, InformControllerResponse


STATE = 'idle'  # Also walk, imitate and fallen


def inform_controller_factory(who):
    def inform_controller(what):
        try:
            inform_controller = rospy.ServiceProxy('inform_controller',
                                                   InformController)
            perm = inform_controller(who, what).permission
        except rospy.service.ServiceException:
            rospy.signal_shutdown('Controller is dead')
            perm = False
        return perm
    return inform_controller


def handle_request(r):
    module = r.module
    message = r.message
    global STATE

    if module == 'walker':
        if message == 'move':
            if STATE in ('idle', 'walk'):
                STATE = 'walk'
                permission = True
            else:
                permission = False
        elif message == 'stop':
            if STATE == 'walk':
                STATE = 'idle'
            permission = True

    elif module == 'fall_detector':
        permission = True
        if message in ('falling', 'fallen'):
            STATE = 'fallen'
        elif message == 'recovered':
            STATE = 'idle'

    elif module == 'imitator':
        if message == 'imitate':
            if STATE == 'imitate':
                permission = True
            else:
                permission = False

    elif module == 'speech_recognition':
        if message == 'imitate':
            if STATE == 'idle':
                STATE = 'imitate'
                permission = True
            else:
                permission = False
        if message == 'stop':
            if STATE == 'imitate':
                STATE = 'idle'
                permission = True
            else:
                permission = False

    rospy.loginfo(
        'Got request from %s to %s. Permission: %s. State is now: %s.' % (
            module, message, permission, STATE
        )
    )
    return InformControllerResponse(permission)


if __name__ == '__main__':
    rospy.init_node('controller', log_level=rospy.INFO)
    ic = rospy.Service('inform_controller', InformController, handle_request)
    rospy.spin()