[Frozen Code Start]
    
[Frozen Code End]

import numpy as np
from scipy.spatial.transform import Rotation as R

def run_skeleton_task():
    '''Task logic to open a drawer and dispose of rubbish in the bin.'''
    print("===== Starting Skeleton Task =====")
    env, task = setup_environment()
    try:
        # Reset and initialize recording
        descriptions, obs = task.reset()
        init_video_writers(obs)
        task.step = recording_step(task.step)
        task.get_observation = recording_get_observation(task.get_observation)

        # Retrieve object positions from the environment
        positions = get_object_positions()

        # Define quaternions for zero and ninety degree rotations about Z axis
        zero_quat = R.from_euler('z', 0, degrees=True).as_quat()
        ninety_quat = R.from_euler('z', 90, degrees=True).as_quat()

        # Step 1: Rotate gripper from 0° to 90°
        print("[Task] Rotating gripper to 90 degrees")
        obs, reward, done = rotate(env, task, ninety_quat)
        if done:
            print("[Task] Early termination after rotate. Reward:", reward)
            return

        # Step 2: Move to side position of bottom drawer
        print("[Task] Moving to bottom side position")
        side_pos = np.array(positions['bottom_side_pos'])
        obs, reward, done = move(env, task, side_pos)
        if done:
            print("[Task] Early termination after move-to-side. Reward:", reward)
            return

        # Step 3: Move to anchor position of bottom drawer
        print("[Task] Moving to bottom anchor position")
        anchor_pos = np.array(positions['bottom_anchor_pos'])
        obs, reward, done = move(env, task, anchor_pos)
        if done:
            print("[Task] Early termination after move-to-anchor. Reward:", reward)
            return

        # Step 4: Grasp the drawer handle (pick-drawer)
        print("[Task] Grasping drawer handle")
        obs, reward, done = pick(env, task, anchor_pos)
        if done:
            print("[Task] Early termination after pick-drawer. Reward:", reward)
            return

        # Step 5: Pull drawer open
        print("[Task] Pulling drawer open")
        # Assume drawer pulls along the +X axis by 0.1 meters
        obs, reward, done = pull(env, task, pull_distance=0.1, pull_axis='x')
        if done:
            print("[Task] Early termination after pull. Reward:", reward)
            return

        # Step 6: Move to the rubbish and pick it up
        print("[Task] Moving to rubbish")
        rubbish_pos = np.array(positions['rubbish'])
        obs, reward, done = move(env, task, rubbish_pos)
        if done:
            print("[Task] Early termination before pick rubbish. Reward:", reward)
            return

        print("[Task] Picking up rubbish")
        obs, reward, done = pick(env, task, rubbish_pos)
        if done:
            print("[Task] Early termination after pick rubbish. Reward:", reward)
            return

        # Step 7: Move to bin and place the rubbish
        print("[Task] Moving to bin")
        bin_pos = np.array(positions['bin'])
        obs, reward, done = move(env, task, bin_pos)
        if done:
            print("[Task] Early termination before place. Reward:", reward)
            return

        print("[Task] Placing rubbish in bin")
        obs, reward, done = place(env, task, bin_pos)
        if done:
            print("[Task] Task completed successfully! Final reward:", reward)
        else:
            print("[Task] Task ended without done=True. Final reward:", reward)

    except Exception as e:
        print(f"[Task] Exception encountered: {e}")
    finally:
        shutdown_environment(env)
    print("===== End of Skeleton Task =====")

if __name__ == "__main__":
    run_skeleton_task()