# import py_trees
import time
from typing import Dict, List

class SceneState:
  def __init__(self, robot_position: List[float], objects: Dict[str, List[float]]) -> None:
    self.robot_position = robot_position
    self.objects = objects
    self.robot_is_holding_object: bool = False
    print(f"Robot starts in {self.robot_position}")
    print(f"Objects: {self.objects}")

  def move_to(self, new_location: List[float]) -> None:
    print(f"Moving to {new_location} from {self.position}")
    time.sleep(2)  # Simulate movement taking 2 seconds
    self.robot_position = new_location

  def pick_up(self) -> None:
    print(f"Picking up object at {self.position}")
    time.sleep(1)  # Simulate picking up taking 1 second
    self.robot_is_holding_object = True  # Assuming we pick up a generic object

  def place_down(self) -> None:
    print(f"Placing object at {self.position}")
    time.sleep(1)  # Simulate placing down taking 1 second
    self.robot_is_holding_object = False

#class MoveTo(py_trees.behaviour.Behaviour):
#    def __init__(self, name, scene_state: SceneState):
#        super(MoveTo, self).__init__(name)
#        self.scene_state = scene_state
#    # TBD ...
# ...

# move_to_pick = MoveTo(name="MoveToPick", scene_state=scene_state)

if __name__=="__main__":
  scene_state = SceneState(robot_position = [1.1,2.2], objects={"item1":[3.3,4.4], "item2":[5.5, 6.6]})