#! /usr/bin/env python
import os

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

from teleoperation.msg import RequestSpeechAction, RequestSpeechResult


speech_broker = None
almem = None


def request_speech(goal):
    rospy.loginfo('SPEECH SERVER: NEW GOAL: {}'.format(goal))
    if not speech_detector.start_speech(goal.vocabulary):
        sas.set_succeeded(RequestSpeechResult(word=''))
        return

    while (not sas.is_preempt_requested() and
           not speech_detector.have_word() and
           not rospy.is_shutdown()):
        rospy.Rate(10).sleep()

    if speech_detector.have_word():
        recognized = speech_detector.get_recognized_and_erase()
    else:
        speech_detector.stop_speech()
        recognized = ''

    sas.set_succeeded(RequestSpeechResult(word=recognized))


class SpeechDetectorModule(ALModule):

    def __init__(self, name):
        ALModule.__init__(self, name)
        self.recognized = None
        self.subid = 'teleoperation_speech'
        self.voc = []
        self.asr = ALProxy('ALSpeechRecognition')
        self.tts = ALProxy('ALTextToSpeech')
        self.asr.setLanguage('English')
        almem.subscribeToEvent("WordRecognized",
                               "speech_detector",
                               "on_word_recognized")
        self.asr.pause(True)
        self._busy = False

    def get_status(self):
        print(almem.getData('ALSpeechRecognition/Status'))

    def start_speech(self, voc, resume=False):
        if self._busy != resume:
            return False
        if not resume:
            self.voc = voc
            self.asr.setVocabulary(voc, False)
            self.asr.subscribe(self.subid)
        self.asr.pause(False)
        self._busy = 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, pause=False):
        if not self._busy:
            return
        self.asr.pause(True)
        if not pause:
            self.asr.unsubscribe(self.subid)
            self._busy = False

    def on_word_recognized(self, *_args):
        word, conf = almem.getData('WordRecognized')
        print(word, conf)
        if conf > 0.4:
            self.stop_speech(pause=True)
            self.tts.say(word)
            self.recognized = word
            self.stop_speech()
        else:
            self.stop_speech(pause=True)
            self.tts.say('nope')
            self.start_speech(self.voc, resume=True)



if __name__ == '__main__':
    rospy.init_node('speech_server')
    speech_broker = ALBroker('speech_broker', '0.0.0.0', 0,
                             os.environ['NAO_IP'], 9559)
    almem = ALProxy('ALMemory')
    speech_detector = SpeechDetectorModule('speech_detector')
    sas = actionlib.SimpleActionServer('speech_server', RequestSpeechAction,
                                       execute_cb=request_speech,
                                       auto_start=False)
    sas.start()

    while not rospy.is_shutdown():
        rospy.Rate(1).sleep()
    while sas.is_active():
        pass

    speech_broker.shutdown()