<mujoco model="origincar_scene">
  <compiler angle="radian" autolimits="true"/>

  <option timestep="0.005" iterations="10" solver="PGS"/>

  <statistic center="0 0 0.1" extent="3"/>

  <visual>
    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
    <rgba haze="0.15 0.25 0.35 1"/>
    <global azimuth="120" elevation="-20"/>
    <quality shadowsize="4096"/>
  </visual>

  <asset>
    <texture name="checker_tex" type="2d" builtin="checker" width="512" height="512" mark="edge"
      rgb1="0.35 0.45 0.55" rgb2="0.55 0.65 0.75" markrgb="0.15 0.15 0.15"/>
    <material name="checker_mat" texture="checker_tex" texrepeat="5 5" texuniform="true" reflectance="0.05"/>

    <mesh name="cone_mesh" file="cone.obj"/>
    <mesh name="cone_white_mesh" file="cone_white.obj"/>
    <texture name="map_tex" type="2d" file="map_full.png"/>
    <material name="map_mat" texture="map_tex" texrepeat="0.2 0.2" texuniform="true" reflectance="0.0"/>

    <texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="512" height="512"/>
  </asset>

  <default>
    <geom density="500"/>
    <joint damping="0.5"/>
    <default class="wheel">
      <geom type="cylinder" size="0.03 0.0125" density="1000" condim="3"/>
      <joint damping="0.2"/>
    </default>
    <default class="steering">
      <joint axis="0 0 1" damping="2" limited="true" range="-0.6 0.6"/>
      <geom type="sphere" size="0.015" density="500" contype="0" conaffinity="0" rgba="0 0 0 0"/>
    </default>
    <default class="cone_blue">
      <geom density="300" rgba="0.0 0.0 0.5 1"/>
    </default>
  </default>

  <worldbody>
    <light name="sun" pos="0 0 5" dir="0 0 -1" directional="true" castshadow="true"
      diffuse="0.8 0.8 0.8" specular="0.2 0.2 0.2"/>

    <!-- Checkerboard ground (infinite) -->
    <geom name="floor" type="plane" size="0 0 0.01" material="checker_mat"
      friction="0.6 0.1 0.1" condim="3"/>

    <!-- 5m x 5m map tile, just above the ground -->
    <geom name="map_tile" type="box" size="2.5 2.5 0.002" pos="0 0 0.002"
      material="map_mat" friction="0.6 0.1 0.1" condim="3"/>

    <!-- 40cm white fence around the map -->
    <!-- ===== Blue cone obstacles (auto-generated from cones.json) ===== -->
    <!-- CONES -->

    <geom name="fence_n" type="box" size="2.5 0.02 0.2" pos="0  2.5 0.2" rgba="1 1 1 1"/>
    <geom name="fence_s" type="box" size="2.5 0.02 0.2" pos="0 -2.5 0.2" rgba="1 1 1 1"/>
    <geom name="fence_e" type="box" size="0.02 2.5 0.2" pos=" 2.5 0 0.2" rgba="1 1 1 1"/>
    <geom name="fence_w" type="box" size="0.02 2.5 0.2" pos="-2.5 0 0.2" rgba="1 1 1 1"/>

    <body name="base_link" pos="0 0 0.08">
      <freejoint/>

      <geom name="base_geom" type="box" size="0.138 0.082 0.04" pos="0 0 0.07"
        rgba="0.4 0.039 0.039 1" contype="0" conaffinity="0"/>

      <!-- Camera: look along +X, right=-Y, up=+Z -->
      <camera name="front_cam" pos="0.1205 0 0.11" xyaxes="0 -1 0 0 0 1" fovy="60"/>

      <body name="camera" pos="0.1205 0 0.11">
        <geom name="camera_geom" type="box" size="0.0075 0.0225 0.015" pos="0 0 0.015"
          rgba="0.365 0.306 0.306 1"/>
      </body>

      <body name="board_link" pos="-0.038 0 0.11">
        <geom name="board_geom" type="box" size="0.100 0.082 0.035" pos="0 0 0.035"
          rgba="0.965 0.596 0.596 1"/>
      </body>

      <!-- ===== Rear wheels (drive only, no steering) ===== -->
      <body name="down_left_Link" pos="-0.0841 0.0945 0.03">
        <joint name="down_left_joint" axis="0 1 0"/>
        <geom class="wheel" zaxis="0 1 0" rgba="0.745 0.129 0.129 1"/>
      </body>

      <body name="down_right_Link" pos="-0.0841 -0.0945 0.03">
        <joint name="down_right_joint" axis="0 -1 0"/>
        <geom class="wheel" zaxis="0 -1 0" rgba="0.745 0.129 0.129 1"/>
      </body>

      <!-- ===== Front wheels (steer + drive) ===== -->
      <body name="up_left_steer" pos="0.0841 0.0945 0.03">
        <joint name="up_left_steer_joint" axis="0 0 1" limited="true" range="-0.6 0.6"/>
        <geom class="steering"/>
        <body name="up_left_Link">
          <joint name="up_left_drive_joint" axis="0 1 0"/>
          <geom class="wheel" zaxis="0 1 0" rgba="0.745 0.129 0.129 1"/>
        </body>
      </body>

      <body name="up_right_steer" pos="0.0841 -0.0945 0.03">
        <joint name="up_right_steer_joint" axis="0 0 1" limited="true" range="-0.6 0.6"/>
        <geom class="steering"/>
        <body name="up_right_Link">
          <joint name="up_right_drive_joint" axis="0 -1 0"/>
          <geom class="wheel" zaxis="0 -1 0" rgba="0.745 0.129 0.129 1"/>
        </body>
      </body>
    </body>
  </worldbody>

  <actuator>
    <!-- Drive motors (torque) for all 4 wheels -->
    <motor name="down_left_drive" joint="down_left_joint" ctrlrange="-20 20"/>
    <motor name="down_right_drive" joint="down_right_joint" ctrlrange="-20 20"/>
    <motor name="up_left_drive" joint="up_left_drive_joint" ctrlrange="-20 20"/>
    <motor name="up_right_drive" joint="up_right_drive_joint" ctrlrange="-20 20"/>

    <!-- Steering position servos (kp/kv matched to car.py pattern) -->
    <position name="up_left_steer" joint="up_left_steer_joint" ctrlrange="-0.6 0.6" kp="3000" kv="300"/>
    <position name="up_right_steer" joint="up_right_steer_joint" ctrlrange="-0.6 0.6" kp="3000" kv="300"/>
  </actuator>
</mujoco>
