<?xml version="1.0"?>
<robot name="car" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- ============================================================
       Sensor switches. Every lesson uses THIS SAME file — earlier
       lessons pass these as :=false, later lessons flip one on at
       a time. Nothing is duplicated per lesson.
       Usage:  xacro car.urdf.xacro enable_lidar:=true
       ============================================================ -->
  <xacro:arg name="enable_lidar" default="false"/>
  <xacro:arg name="enable_camera" default="false"/>
  <xacro:arg name="enable_imu" default="false"/>

  <xacro:property name="M_PI" value="3.14159265359"/>

  <!-- Chassis -->
  <xacro:property name="chassis_x" value="0.4"/>
  <xacro:property name="chassis_y" value="0.3"/>
  <xacro:property name="chassis_z" value="0.15"/>
  <xacro:property name="chassis_mass" value="5.0"/>

  <!-- Wheels -->
  <xacro:property name="wheel_radius" value="0.08"/>
  <xacro:property name="wheel_length" value="0.04"/>
  <xacro:property name="wheel_mass" value="0.5"/>
  <xacro:property name="wheel_separation" value="0.35"/>
  <xacro:property name="wheelbase" value="0.32"/>

  <material name="car_red">
    <color rgba="0.75 0.1 0.1 1.0"/>
  </material>
  <material name="wheel_black">
    <color rgba="0.05 0.05 0.05 1.0"/>
  </material>
  <material name="sensor_grey">
    <color rgba="0.3 0.3 0.3 1.0"/>
  </material>

  <!-- ============================================================
       Chassis. base_link IS the chassis (no separate dummy root) —
       this is also the frame the DiffDrive plugin drives around as
       its child_frame_id, and the root of the robot's TF tree.

       Note (deliberate simplification): a rigorous REP-105 setup
       adds a base_footprint link below base_link. We skip it here —
       fine for a flat-ground sim car, see car_gazebo Lesson 11 README.
       ============================================================ -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${chassis_x} ${chassis_y} ${chassis_z}"/>
      </geometry>
      <material name="car_red"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${chassis_x} ${chassis_y} ${chassis_z}"/>
      </geometry>
    </collision>
    <inertial>
      <!-- Solid box inertia: Ixx=m(y^2+z^2)/12, Iyy=m(x^2+z^2)/12, Izz=m(x^2+y^2)/12 -->
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${chassis_mass}"/>
      <inertia ixx="0.046875" ixy="0.0" ixz="0.0" iyy="0.076042" iyz="0.0" izz="0.104167"/>
    </inertial>
  </link>

  <!-- ============================================================
       Wheels. One macro, called 4 times — front wheels are passive
       (continuous joint, no drive plugin references them), rear
       wheels are the driven pair named in the DiffDrive plugin
       below. That makes this a rear-wheel-drive car, not a diff-
       drive robot with two "extra" decorative wheels.
       ============================================================ -->
  <xacro:macro name="wheel" params="prefix x_offset y_offset">
    <link name="${prefix}_wheel_link">
      <visual>
        <!-- Cylinder's native axis is Z; rotating 90 deg about X lays
             it on its side so it spins about Y, like a real wheel. -->
        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
        </geometry>
        <material name="wheel_black"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
        </geometry>
      </collision>
      <inertial>
        <!-- Cylinder inertia, symmetry axis = Y here (see rotation above):
             Iyy (about spin axis) = m*r^2/2 = 0.0016
             Ixx = Izz (perpendicular)  = m*(3r^2+h^2)/12 = 0.000867
             Getting this on the wrong axis is a classic bug — see the
             Common Mistakes table below. -->
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${wheel_mass}"/>
        <inertia ixx="0.000867" ixy="0.0" ixz="0.0" iyy="0.0016" iyz="0.0" izz="0.000867"/>
      </inertial>
    </link>

    <joint name="${prefix}_wheel_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_wheel_link"/>
      <origin xyz="${x_offset} ${y_offset} ${-chassis_z/2}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>

  <xacro:wheel prefix="front_left"  x_offset="${wheelbase/2}"  y_offset="${wheel_separation/2}"/>
  <xacro:wheel prefix="front_right" x_offset="${wheelbase/2}"  y_offset="${-wheel_separation/2}"/>
  <xacro:wheel prefix="rear_left"   x_offset="${-wheelbase/2}" y_offset="${wheel_separation/2}"/>
  <xacro:wheel prefix="rear_right"  x_offset="${-wheelbase/2}" y_offset="${-wheel_separation/2}"/>

  <!-- ============================================================
       Sensor mount links. These are always physically present from
       Lesson 10 onward (small, lightweight housings, fixed to the
       chassis) — only the <sensor> tags inside them (further down)
       are gated by the enable_* switches. Inertia here is a small
       placeholder: each housing is light enough, and rigidly fixed,
       that its exact inertia barely affects the whole car's physics.
       ============================================================ -->
  <link name="lidar_link">
    <visual>
      <geometry><cylinder radius="0.035" length="0.04"/></geometry>
      <material name="sensor_grey"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.035" length="0.04"/></geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="lidar_joint" type="fixed">
    <parent link="base_link"/>
    <child link="lidar_link"/>
    <origin xyz="${chassis_x/2 - 0.03} 0 ${chassis_z/2 + 0.03}" rpy="0 0 0"/>
  </joint>

  <link name="camera_link">
    <visual>
      <geometry><box size="0.02 0.06 0.02"/></geometry>
      <material name="sensor_grey"/>
    </visual>
    <collision>
      <geometry><box size="0.02 0.06 0.02"/></geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="${chassis_x/2} 0 ${chassis_z/2 - 0.02}" rpy="0 0 0"/>
  </joint>

  <link name="imu_link">
    <visual>
      <geometry><box size="0.03 0.03 0.01"/></geometry>
      <material name="sensor_grey"/>
    </visual>
    <collision>
      <geometry><box size="0.03 0.03 0.01"/></geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 ${chassis_z/2 + 0.005}" rpy="0 0 0"/>
  </joint>

  <!-- ============================================================
       Model-level Gazebo plugins. These live in the ROBOT's own
       <gazebo> block (no "reference" attribute = applies to the
       whole model) — contrast with the world-level Sensors/Imu
       system plugins added in car_gazebo's world files later.
       ============================================================ -->
  <gazebo>
    <plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
      <left_joint>rear_left_wheel_joint</left_joint>
      <right_joint>rear_right_wheel_joint</right_joint>
      <wheel_separation>${wheel_separation}</wheel_separation>
      <wheel_radius>${wheel_radius}</wheel_radius>
      <odom_publish_frequency>30</odom_publish_frequency>
      <topic>cmd_vel</topic>
      <odom_topic>odom</odom_topic>
      <tf_topic>tf</tf_topic>
      <frame_id>odom</frame_id>
      <child_frame_id>base_link</child_frame_id>
    </plugin>
    <plugin filename="gz-sim-joint-state-publisher-system" name="gz::sim::systems::JointStatePublisher">
      <topic>joint_states</topic>
    </plugin>
  </gazebo>

  <!-- ============================================================
       Sensors — each block only exists in the compiled URDF if its
       enable_* arg is true. Un-gate one lesson at a time.
       ============================================================ -->
  <xacro:if value="$(arg enable_lidar)">
    <gazebo reference="lidar_link">
      <sensor name="lidar" type="gpu_lidar">
        <update_rate>10</update_rate>
        <topic>scan</topic>
        <gz_frame_id>lidar_link</gz_frame_id>
        <always_on>true</always_on>
        <visualize>true</visualize>
        <lidar>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1</resolution>
              <min_angle>-3.14159265</min_angle>
              <max_angle>3.14159265</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.12</min>
            <max>10.0</max>
            <resolution>0.01</resolution>
          </range>
        </lidar>
      </sensor>
    </gazebo>
  </xacro:if>

  <xacro:if value="$(arg enable_camera)">
    <gazebo reference="camera_link">
      <sensor name="camera" type="camera">
        <update_rate>15</update_rate>
        <topic>camera/image_raw</topic>
        <gz_frame_id>camera_link</gz_frame_id>
        <always_on>true</always_on>
        <visualize>true</visualize>
        <camera>
          <horizontal_fov>1.1</horizontal_fov>
          <image>
            <width>640</width>
            <height>480</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.05</near>
            <far>50.0</far>
          </clip>
        </camera>
      </sensor>
    </gazebo>
  </xacro:if>

  <xacro:if value="$(arg enable_imu)">
    <gazebo reference="imu_link">
      <sensor name="imu" type="imu">
        <update_rate>50</update_rate>
        <topic>imu</topic>
        <gz_frame_id>imu_link</gz_frame_id>
        <always_on>true</always_on>
      </sensor>
    </gazebo>
  </xacro:if>

</robot>
