import numpy as np
from agentspace import space, Agent
from pyicubsim import iCubBall, iCubRightArm, iCubTorso, hit
from llm import embed_and_encode, decode_and_wipeout, cosine_similarity
import time

right_arm = iCubRightArm()
ball = iCubBall()
torso = iCubTorso()

class ControlAgent(Agent):

    def recognize(self, text, command):
        return text.startswith(command)

    def response(self,question):
        return question # parroting
            
    def init(self):
        space.attach_trigger("entered",self)
        
    def senseSelectAct(self):
        text = space(default="")["entered"]

        if self.recognize(text,'hit'):
            goal = ball.get() 
            possible = hit(torso,right_arm,goal,False)
            if possible:
                time.sleep(2)
                right_arm.reset()
                torso.reset()
                time.sleep(2)
            else:
                space['text-to-speak'] = 'It is not possible'
            return
        elif self.recognize(text,'say '):
            space['text-to-speak'] = text[4:]
        elif self.recognize(text,'smile'):
            space['smile'] = True
        else:
            space['text-to-speak'] = self.response(text)
        
