import rclpy
from rclpy.node import Node
from defs.srv import HighLvlCmds
from high_lvl_cmd_srv.cmd_constants import *

import random
from enum import Enum
from pocketsphinx import LiveSpeech
from word2number import w2n



WakeWord        = 'hey ar3'
LanguageModel   = '/ros_root/Control-Software/src/voice_move/voice_move/resources/ar3.lm'
Dict            = '/ros_root/Control-Software/src/voice_move/voice_move/resources/ar3.dict'

ConfirmWords    = ['yes', 'confirm', 'go ahead', 'do it']
DenyWords       = ['no', 'deny', 'stop', 'wait', 'never mind', 'ignore', 'hold on']

VoiceCmdWords   = { Cmd.IGNORE.value        : ['ignore', 'never mind', 'wait'],
                    Cmd.OPEN.value          : ['drop', 'let it', 'let go', 'open', 'release'],
                    Cmd.CLOSE.value         : ['close', 'grab', 'grasp', 'take'],
                    Cmd.MOVE.value          : ['move', 'shift', 'go to', 'position'],
                    Cmd.TURN.value          : ['turn', 'rotate'],
                    Cmd.PICKUP.value        : ['pick', 'get'],
                    Cmd.SETDOWN.value       : ['set', 'put', 'leave'],
                    Cmd.GIVE.value          : ['give', 'gimme', 'hand the', 'hand it', 'hand me', 'pass'] }

ItemWords       = { Item.CAN.value          : ['a can', 'the can', 'that can', 'this can', 'soda can'],
                    Item.BLOCK.value        : ['block', 'cube'],
                    Item.BALL.value         : ['ball', 'sphere'] }

DirectionWords  = { Direction.LEFT.value    : ['left', 'clockwise'],
                    Direction.RIGHT.value   : ['right', 'counter clockwise'],
                    Direction.UP.value      : ['up'],
                    Direction.DOWN.value    : ['down'] }


# Used for choosing which set of word lists to use for word parsing
EnumType = [Cmd, Item, Direction]
Words = [VoiceCmdWords, ItemWords, DirectionWords]
class WordsType(Enum):
    CMD         = 0
    ITEM        = 1
    DIRECTION   = 2
    POSITION    = 3
    DEGREES     = 4



class VoiceListener(Node):

    def __init__(self):

        self.wakeWordSpoken = False

        self.speech = LiveSpeech(
            audio_device='plughw:CARD=Generic_1,DEV=0',
            lm=LanguageModel,
            dic=Dict
        )

        super().__init__('voice_move')

        self.logger = self.get_logger()
        self.logger.info('voice_listener init')

        self.cli = self.create_client(HighLvlCmds, 'high_lvl_cmds')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.logger.info('service not available, waiting again...')

        self.req = HighLvlCmds.Request()

        self._timer = self.create_timer(0.01, self._listen)


    def _listen(self):

        if self.wakeWordSpoken:
            self.logger.info(random.choice(['Yes?', 'Go on.', 'Go ahead.']))
        else:
            self.logger.info('Listening...')

        for phrase in self.speech:

            if self.wakeWordSpoken:

                phraseStr = str(phrase)

                self.logger.info(f"Heard '{phraseStr}'")

                command     = self.parseWords(phraseStr, WordsType.CMD)
                item        = Item.NONE
                direction   = Direction.NONE
                position    = -1
                degrees     = -1

                if command == Cmd.PICKUP or command == Cmd.SETDOWN or command == Cmd.GIVE:
                    item = self.parseWords(phraseStr, WordsType.ITEM)
                
                if command == Cmd.MOVE:
                    direction = self.parseWords(phraseStr, WordsType.DIRECTION)
                    degrees = self.parseWords(phraseStr, WordsType.DEGREES)

                self.req.command        = command.value
                self.req.item           = item.value
                self.req.direction      = direction.value
                self.req.position       = position
                self.req.degrees        = degrees

                self.future = self.cli.call_async(self.req)

                self.wakeWordSpoken = False

            else:

                if WakeWord in str(phrase):
                    # Wait for the command in the next phrase
                    self.wakeWordSpoken = True

            return

    def parseWords(self, phrase, wordsType):

        if wordsType == WordsType.POSITION or wordsType == WordsType.DEGREES:
            try:
                return w2n.word_to_num(phrase.replace('a hundred', 'one hundred'))
            except Exception:
                return -1
                # raise MoveError('Failed to interpret number of degrees')

        theWords = Words[wordsType.value]
        theEnum = EnumType[wordsType.value]

        for i in range(len(theWords)):
            wordList = theWords[i]
            for word in wordList:
                if word in phrase:
                    return theEnum(i)

        return theEnum.NONE

def main(args=None):

    rclpy.init(args=args)
    voice_listener = VoiceListener()
    rclpy.spin(voice_listener)

    voice_listener.destroy_node()
    rclpy.shutdown


if __name__ == '__main__':
    main()
