#! /usr/bin/env python
import os

import rospy
import actionlib
from naoqi import ALProxy, ALModule, ALBroker


speech_broker = None
almem = None
sas = actionlib.SimpleActionServer()


def request_speech(goal):
    recognized = ''
    if not speech_detector.start_speech(goal):
        sas.publish_feedback(False)
        return
    while not sas.is_preempt_requested() and not speech_detector.have_word():
        rospy.Rate(10).sleep()
    speech_detector.stop_speech(goal)
    if speech_detector.have_word():
        recognized = speech_detector.get_recognized_and_erase()
    sas.publish_feedback(True)
    sas.set_succeeded(recognized)


class SpeechDetectorModule(ALModule):

    def __init__(self, name):
        ALModule.__init__(self, name)
        self.recognized = None
        self.subid = 'teleoperation_speech'
        self.asr = ALProxy('ALSpeechRecognition')
        self.asr.setLanguage('English')
        self.stop_speech()

    def start_speech(self, voc):
        if self.running:
            return False
        # self.stop_speech()
        self.asr.setVocabulary(voc, False)
        self.asr.subscribe(self.subid)
        self.asr.pause(False)
        self.running = True
        return True

    def have_word(self):
        return self.recognized is not None

    def get_recognized_and_erase(self):
        result = self.recognized
        self.recognized = None
        return result

    def stop_speech(self):
        self.asr.pause(True)
        self.asr.unsubscribe(self.subid)
        self.running = False

    def on_word_recognized(self, *_args):
        rospy.loginfo(almem.getData('WordRecognized'))


if __name__ == '__main__':
    rospy.init_node('speech')
    speech_broker = ALBroker('speech_broker', '0.0.0.0', 0,
                             os.environ['NAO_IP'], 9559)
    speech_detector = SpeechDetectorModule('speech_detector')
    almem = ALProxy('ALMemory')
    almem.subscribeToEvent("WordRecognized",
                           "speech_detector",
                           "on_word_recognized")

    speech_detector.start_speech(['start', 'stop', 'pause'])

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

    speech_detector.stop_speech()
    speech_broker.shutdown()