#! /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
r = False


def request_speech(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():
        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')
        self.running = False

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

    def on_word_recognized(self, *_args):
        word, conf = almem.getData('WordRecognized')
        if conf > 0.4:
            self.stop_speech()
            self.tts.say(word)
            self.recognized = word
        else:
            self.stop_speech()
            self.tts.say('I didn\'t understand. Please repeat')
            self.start_speech(self.voc)



if __name__ == '__main__':
    rospy.init_node('speech_server')
    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.asr.pause(True)
    sas = actionlib.SimpleActionServer('speech_server', RequestSpeechAction,
                                       execute_cb=request_speech,
                                       auto_start=False)
    sas.start()

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

    if speech_detector.running:
        speech_detector.stop_speech()

    speech_broker.shutdown()