# For Pi0 or Pi0.5
pip install -e ".[pi]"

# For SmolVLA
pip install -e ".[smolvla]"

from lerobot.policies.pi0 import PI0Policy, PI0Config
from lerobot.configs.types import RTCAttentionSchedule
from lerobot.policies.rtc.configuration_rtc import RTCConfig
from lerobot.policies.rtc.action_queue import ActionQueue

# Load Pi0 with RTC enabled
policy_cfg = PI0Config()

# Enable RTC
policy_cfg.rtc_config = RTCConfig(
    enabled=True,
    execution_horizon=10,  # How many steps to blend with previous chunk
    max_guidance_weight=10.0,  # How strongly to enforce consistency
    prefix_attention_schedule=RTCAttentionSchedule.EXP,  # Exponential blend
)

# Load the policy
policy = PI0Policy.from_pretrained("lerobot/pi0_base", policy_cfg=policy_cfg, device="cuda")

# Now use predict_action_chunk with RTC parameters
inference_delay = 4  # How many steps of inference latency, this values should be calculated based on the inference latency of the policy

# Initialize the action queue
action_queue = ActionQueue(policy_cfg.rtc_config)

# Start in a separate thread with the following function
def get_actions():
  while True:
    if should_get_actions:

      prev_actions = action_queue.get_left_over()
      obs = get_robot_observations(robot)

      # Generate actions WITH RTC
      actions = policy.predict_action_chunk(
          obs,
          inference_delay=inference_delay,
          prev_chunk_left_over=prev_actions,
      )

      action_queue.merge(
          actions, actions, inference_delay
      )

for step in range(num_steps):
    action = action_queue.get()

    # Execute the first N actions
    execute_actions(action)

RTCConfig(execution_horizon=10)

python examples/rtc/eval_dataset.py \
    --policy.path=lerobot/pi0_libero_finetuned \
    --dataset.repo_id=HuggingFaceVLA/libero \
    --rtc.execution_horizon=10 \
    --rtc.max_guidance_weight=10.0 \
    --device=cuda

python examples/rtc/eval_with_real_robot.py \
    --policy.path=${HF_USERNAME}/policy_repo_id \
    --robot.type=so100_follower \
    --robot.port=/dev/tty.usbmodem58FA0834591 \
    --robot.cameras="{ gripper: {type: opencv, index_or_path: 1, width: 640, height: 480, fps: 30}, front: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 30}}" \
    --task="Move green small object into the purple platform" \
    --duration=120 \
    --device=cuda

# Enable debug tracking
policy_cfg.rtc_config.debug = True
policy_cfg.rtc_config.debug_maxlen = 100

# After inference, access debug data
debug_data = policy.rtc_processor.get_debug_data()

# Visualize denoising steps, corrections, etc.
from lerobot.policies.rtc.debug_visualizer import RTCDebugVisualizer
visualizer = RTCDebugVisualizer()
# ... create plots