from aim_fsm import *

class Calibrate(StateMachineProgram):
    def __init__(self):
        super().__init__(wall_marker_dict=None)

    def start(self):
        self.robot.world_map.clear()
        super().start()

    class Report(StateNode):
        def start(self, event=None):
            marker3 = self.robot.aruco_detector.seen_marker_objects[3]
            corners = marker3.corners
            print('Aruco marker 3 corners:')
            print(corners)
            # *** FILL IN THE MISSING PARTS TO CALCULATE DISTANCE ***
            distance = None
            print(f'Distance to marker 3 bottom edge is {distance}')

    $setup{
        wait_for_vision: StateNode() =T(0.5)=> self.Report()
    }