"""
Park at Start — loads a track and holds the car at the starting line for screenshots.

Usage:
    python3 park_at_start.py --track generated_road
    python3 park_at_start.py --track generated_track
    python3 park_at_start.py --track mountain_track
    python3 park_at_start.py --track warehouse
    python3 park_at_start.py --track sparkfun_avc
    python3 park_at_start.py --track mini_monaco
    python3 park_at_start.py --track warren
    python3 park_at_start.py --track roboracingleague_1
    python3 park_at_start.py --track waveshare
    python3 park_at_start.py --track circuit_launch

The car will sit still at the starting line.
Press Ctrl+C when done with screenshot. Script will clean up automatically.
"""

import time
import sys
import argparse
import gymnasium as gym
import gym_donkeycar
from track_switcher import switch_track

TRACK_MAP = {
    'generated_road':     'donkey-generated-roads-v0',
    'generated_track':    'donkey-generated-track-v0',
    'mountain_track':     'donkey-mountain-track-v0',
    'warehouse':          'donkey-warehouse-v0',
    'sparkfun_avc':       'donkey-avc-sparkfun-v0',
    'mini_monaco':        'donkey-minimonaco-track-v0',
    'warren':             'donkey-warren-track-v0',
    'roboracingleague_1': 'donkey-roboracingleague-track-v0',
    'waveshare':          'donkey-waveshare-v0',
    'circuit_launch':     'donkey-circuit-launch-track-v0',
}

parser = argparse.ArgumentParser()
parser.add_argument('--track', required=True, choices=list(TRACK_MAP.keys()),
                    help='Track name to load')
parser.add_argument('--current', default='donkey-generated-roads-v0',
                    help='Current env ID (to exit from)')
args = parser.parse_args()

env_id = TRACK_MAP[args.track]
print(f'\n[Park] Loading track: {args.track}  ({env_id})', flush=True)

env = switch_track(target_env_id=env_id, current_env_id=args.current)
obs, info = env.reset()

print(f'\n[Park] ✅ Car is at the starting line on: {args.track}', flush=True)
print(f'[Park] Take your screenshot now!', flush=True)
print(f'[Park] Press Ctrl+C when done.\n', flush=True)

# Hold the car still with zero throttle/steer
try:
    import numpy as np
    while True:
        action = env.action_space.sample() * 0.0  # all zeros = parked
        env.step(action)
        time.sleep(0.1)
except KeyboardInterrupt:
    print('\n[Park] Exiting...', flush=True)
finally:
    env.unwrapped.viewer.exit_scene()
    time.sleep(1)
    env.close()
    print('[Park] Done. Sim returned to main menu.', flush=True)