#! /usr/bin/env python
import rospy
import actionlib

from teleoperation.srv import InformController, InformControllerResponse
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal


STATE = 'dead'  # Also walk, imitate, fallen, idle
speech_in_progress = False

IMITATE = 'arms'
KILL = 'kill'
REVIVE = 'go'
STOP = 'stop'

voc_state = {
    'idle': [IMITATE, KILL],
    'imitate': [STOP, KILL],
    'dead': [REVIVE]
}


def speech_done_cb(_, result):
    global speech_in_progress, STATE
    _state_old = STATE
    rospy.loginfo('SPEECH: {}'.format(result))
    if not result:
        speech_in_progress = False
        return
    if result.word == IMITATE and STATE == 'idle':
        STATE = 'imitate'
    elif result.word == STOP and STATE == 'imitate':
        STATE = 'idle'
    elif result.word == KILL and STATE in ('idle', 'imitate'):
        STATE = 'dead'
    elif result.word == REVIVE and STATE == 'dead':
        STATE = 'idle'
    if _state_old != STATE:
        rospy.loginfo('{} -> {}'.format(_state_old, STATE))
    speech_in_progress = False


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):
    global STATE
    module = r.module
    message = r.message
    _state_old = STATE
    permission = False

    if module == 'walker':
        if message == 'move':
            if STATE in ('idle', 'walk'):
                STATE = 'walk'
                permission = True
        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

    rospy.logdebug(
        'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nSTATE IS NOW: %s.' % (
            module, message, permission, STATE
        )
    )
    if _state_old != STATE:
        rospy.loginfo('{} -> {}'.format(_state_old, STATE))
    return InformControllerResponse(permission)


if __name__ == '__main__':

    rospy.init_node('controller', log_level=rospy.INFO)
    ic = rospy.Service('inform_controller', InformController, handle_request)

    client = actionlib.SimpleActionClient('speech_server',
                                          RequestSpeechAction)
    client.wait_for_server()
    rospy.loginfo('SPEECH: SERVER THERE')

    rospy.on_shutdown(lambda: speech_in_progress and client.cancel_goal())

    while not rospy.is_shutdown():
        if not STATE in ('idle', 'imitate', 'dead'):
            if speech_in_progress:
                client.cancel_goal()
                speech_in_progress = False
        else:
            if not speech_in_progress:
                speech_in_progress = True
                client.send_goal(RequestSpeechGoal(voc_state[STATE]),
                                 speech_done_cb)
        rospy.Rate(10).sleep()