from cozmo_fsm import *
from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes


def handle_object_appeared(evt, **kw):
    # This will be called whenever an EvtObjectAppeared is dispatched -
    # whenever an Object comes into view.
    global EV
    print('handle')
    if isinstance(evt.obj, CustomObject):
        print("Cozmo started seeing a %s" % str(evt.obj.object_type))
        EV = evt

async def declare_objects():
    cube_obj = await robot.world.define_custom_cube(CustomObjectTypes.CustomType05,
                                              CustomObjectMarkers.Circles2,
                                              50,
                                              40, 40, True)
    print('cube_obj=',cube_obj)
    global CU
    CU = cube_obj

class Qube(StateMachineProgram):
    def start(self):
        super().start()
        print('starting')
        robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared)
        robot.loop.create_task(declare_objects())
        print('started')