#! /usr/bin/env python
from argparse import ArgumentParser
from sys import argv

import rospy
import actionlib

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


# Constants
IMITATE = 'arms'
KILL = 'kill'
REVIVE = 'go'
STOP = 'stop'
VOC_STATE = None
SPEECH_TRANSITIONS = None
AI = False  # autoimitate


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


def init_voc_state_speech():
    global VOC_STATE, SPEECH_TRANSITIONS
    VOC_STATE = {
        'idle': [KILL] + ([IMITATE] if not AI else []),
        'imitate': [KILL] + ([STOP] if not AI else []),
        'dead': [REVIVE]
    }
    SPEECH_TRANSITIONS = {
        KILL: (('idle', 'imitate'), 'dead'),
        REVIVE: (('dead',), 'idle')
    }
    if not AI:
        SPEECH_TRANSITIONS.update({
            IMITATE: (('idle',), 'imitate'),
            STOP: (('imitate',), 'idle')
        })


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 in SPEECH_TRANSITIONS:
        allowed, target = SPEECH_TRANSITIONS[result.word]
        if state in allowed:
            state = target

    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') + (('imitate',) if AI else ()):
                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 in ('imitate',) + (('idle',) if AI else ()):
                permission = True
                state = 'imitate'

    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__':

    print(argv)
    ap = ArgumentParser()
    ap.add_argument('-i', '--autoimitate',
                    help='Switch between moving and imitating automatically',
                    action='store_true')
    args, _ = ap.parse_known_args()

    AI = args.autoimitate
    init_voc_state_speech()

    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 state in ('idle', 'imitate', 'dead'):
            if not speech_in_progress:
                speech_in_progress = True
                client.send_goal(RequestSpeechGoal(VOC_STATE[state]),
                                 speech_done_cb)
        else:
            if speech_in_progress:
                client.cancel_goal()
                speech_in_progress = False

        rospy.Rate(10).sleep()