[Frozen Code Start]
    
[Frozen Code End]

def run_skeleton_task():
    '''Generic skeleton for running any task in your simulation.'''
    print("===== Starting Skeleton Task =====")
    
    # === Environment Setup ===
    env, task = setup_environment()
    try:
        # Reset the task to its initial state
        descriptions, obs = task.reset()

        # (Optional) Initialize video writers for capturing your simulation
        init_video_writers(obs)

        # Wrap the task steps for recording (if needed)
        original_step = task.step
        task.step = recording_step(original_step)
        original_get_obs = task.get_observation
        task.get_observation = recording_get_observation(original_get_obs)

        # === Retrieve Object Positions ===
        positions = get_object_positions()

        # Map the PDDL object names to keys in the positions dict
        # Try both dashed and underscore forms, and also bottom_<> ordering
        def lookup(name, alt=None):
            if name in positions:
                return positions[name]
            u = name.replace('-', '_')
            if u in positions:
                return positions[u]
            if alt and alt in positions:
                return positions[alt]
            return None

        side_bottom = lookup('side-pos-bottom', 'bottom_side_pos')
        anchor_bottom = lookup('anchor-pos-bottom', 'bottom_anchor_pos')
        rubbish_pos = lookup('rubbish')
        bin_pos = lookup('bin')

        if side_bottom is None or anchor_bottom is None or rubbish_pos is None or bin_pos is None:
            raise KeyError(f"Missing required object positions: "
                           f"side_bottom={side_bottom}, anchor_bottom={anchor_bottom}, "
                           f"rubbish_pos={rubbish_pos}, bin_pos={bin_pos}")

        # === Oracle Plan Execution ===

        # Step 1: Rotate gripper from zero to ninety degrees around its local z-axis
        print("[Task] Step 1: Rotating gripper by 90 degrees around Z")
        # We compute the target quaternion by composing a 90° rotation about Z with the current orientation
        from scipy.spatial.transform import Rotation as R
        obs0 = task.get_observation()
        current_quat = normalize_quaternion(obs0.gripper_pose[3:7])
        current_rot = R.from_quat(current_quat)
        rot_z90 = R.from_euler('z', 90, degrees=True)
        target_rot = rot_z90 * current_rot
        target_quat = target_rot.as_quat()
        obs, reward, done = rotate(env, task, target_quat)
        if done:
            print("[Task] Terminated during rotation.")
            return

        # Step 2: Move gripper to the side-handle position of the bottom drawer
        print(f"[Task] Step 2: Moving to side position: {side_bottom}")
        obs, reward, done = move(env, task, target_pos=side_bottom)
        if done:
            print("[Task] Terminated during move to side position.")
            return

        # Step 3: Move gripper to the anchor-handle position of the bottom drawer
        print(f"[Task] Step 3: Moving to anchor position: {anchor_bottom}")
        obs, reward, done = move(env, task, target_pos=anchor_bottom)
        if done:
            print("[Task] Terminated during move to anchor position.")
            return

        # Step 4: Pull the drawer open by the distance between anchor and side
        import numpy as np
        pull_distance = np.linalg.norm(side_bottom - anchor_bottom)
        print(f"[Task] Step 4: Pulling drawer by distance {pull_distance:.3f}")
        obs, reward, done = pull(env, task, pull_distance=pull_distance, pull_axis='x')
        if done:
            print("[Task] Terminated during pull.")
            return

        # Step 5: Pick up the rubbish from the table
        print(f"[Task] Step 5: Picking up rubbish at {rubbish_pos}")
        obs, reward, done = pick(env, task, target_pos=rubbish_pos)
        if done:
            print("[Task] Terminated during pick.")
            return

        # Step 6: Place the rubbish into the bin
        print(f"[Task] Step 6: Placing rubbish into bin at {bin_pos}")
        obs, reward, done = place(env, task, target_pos=bin_pos)
        if done:
            print("[Task] Task completed successfully with done=True. Reward:", reward)
        else:
            print("[Task] Finished last step. done flag is False. Reward:", reward)

    except Exception as e:
        print(f"[Task] Exception encountered: {e}")
        raise
    finally:
        # Always ensure the environment is properly shutdown
        shutdown_environment(env)
        print("===== End of Skeleton Task =====")


if __name__ == "__main__":
    run_skeleton_task()