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

  <xacro:macro name="link_base" params="name mesh_file mesh_scale">
      <link name="${name}">
          <inertial>
            <mass value="10.2"/>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
          </inertial>
          <collision name="${name}_collision">
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
              <mesh filename="package://wall/meshes/${mesh_file}" scale="${mesh_scale}"/>
            </geometry>
          </collision>
          <visual name="${name}_visual">
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
              <mesh filename="package://wall/meshes/${mesh_file}" scale="${mesh_scale}"/>
            </geometry>
          </visual>
      </link>
  </xacro:macro>  

  <xacro:macro name="link_shaft" params="name mesh_file mesh_scale origin_xyz">
      <link name="${name}">
          <inertial>
            <mass value="0.2"/>
            <origin rpy="0 0 0" xyz="${origin_xyz}"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
          </inertial>
          <collision name="${name}_collision">
            <origin rpy="0 0 0" xyz="${origin_xyz}"/>
            <geometry>
              <mesh filename="package://wall/meshes/${mesh_file}" scale="${mesh_scale}"/>
            </geometry>
          </collision>
          <visual name="${name}_visual">
            <origin rpy="0 0 0" xyz="${origin_xyz}"/>
            <geometry>
              <mesh filename="package://wall/meshes/${mesh_file}" scale="${mesh_scale}"/>
            </geometry>
          </visual>
      </link>
  </xacro:macro>

  <xacro:macro name="joint_shaft" params="name parent child origin_xyz">
    <joint name="${name}" type="prismatic">
      <origin rpy="0 0 0" xyz="${origin_xyz}"/>
      <child link="${child}"/>
      <parent link="${parent}"/>
      <axis rpy="0 0 0" xyz="0 0 1"/>
      <limit effort="1" lower="-0.05" upper="0" velocity="0.1"/>
    </joint>
   </xacro:macro>

   <xacro:macro name="link_plate" params="name mesh_file mesh_scale origin_xyz rotation">
      <link name="${name}">
          <inertial>
            <mass value="1.2"/>
            <origin rpy="${rotation}" xyz="${origin_xyz}"/>
            <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
          </inertial>
          <collision name="${name}_collision">
            <origin rpy="${rotation}" xyz="${origin_xyz}"/>
            <geometry>
              <mesh filename="package://wall/meshes/${mesh_file}" scale="${mesh_scale}"/>
              <!-- <mesh filename="package://wall/meshes/half_plate_l.dae"/> -->
            </geometry>
          </collision>
          <visual name="${name}_visual">
            <origin rpy="${rotation}" xyz="${origin_xyz}"/>
            <geometry>
              <mesh filename="package://wall/meshes/${mesh_file}" scale="${mesh_scale}"/>
              <!-- <mesh filename="package://wall/meshes/half_plate_l.dae"/> -->
            </geometry>
          </visual>
      </link>
  </xacro:macro>
   <xacro:macro name="link_ball" params="name">
      <link name="${name}">
          <inertial>
            <mass value="0.02"/>
            <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
          </inertial>
      </link>
   </xacro:macro>
   <xacro:macro name="joint_plate" params="name parent child link_1 link_2 origin_xyz rotation_1 rotation_2">
      <joint name="${name}_joint_0" type="fixed" >
        <origin xyz="${origin_xyz}" />
        <parent link="${parent}" />
        <child link="${link_1}" />
      </joint>
      <joint name="${name}_joint_1" type="revolute" >
        <origin xyz="0 0 0.0001" />
        <parent link="${link_1}" />
        <child link="${link_2}" />
        <axis xyz="${rotation_1}" />
        <dynamics damping="0.005" />
        <limit effort="1" lower="${-pi/3}" upper="${pi/3}" velocity="0.1"/>
      </joint>
      <joint name="${name}_joint_2" type="revolute" >
        <origin xyz="0 0 0.0001" />
        <parent link="${link_2}"/>
        <child link="${child}"/>
        <axis xyz="${rotation_2}" />
        <dynamics damping="0.005" />
        <limit effort="1" lower="${-pi/4}" upper="${pi/4}" velocity="0.1"/>
      </joint>
   </xacro:macro>

  <xacro:macro name="wall_addon_macro"
    params="namespace name stiffness ball_joint_limits_v ball_joint_limits_h parent *origin">
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}"/>
   </joint>  

   <link name="${name}">
      <visual>
        <origin rpy="0 0 0" xyz="0 0 -0.015"/>
        <geometry>
          <box size="0.03 0.8 0.03"/>
        </geometry>
      </visual>
  </link> 

  <joint name="${name}_base_joint" type="fixed">
      <origin rpy="0 0 0" xyz="0 -0.4 0"/>
      <parent link="${name}"/>
      <child link="${name}_base"/>
   </joint>  
   <link name="${name}_base">
      <inertial>
        <mass value="0.228"/>
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <inertia ixx="306e-06" ixy="-13e-09" ixz="-1e-09" iyy="1197e-06" iyz="258e-09" izz="1192e-06"/>
      </inertial>
      <visual name="${name}_base_visual">
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://rotors_description/meshes/base_wall.dae" scale="1 1 1"/>
        </geometry>
      </visual>
  </link>

  <joint name="${name}_comp_joint" type="fixed">
      <origin rpy="0 0 0" xyz="0 0.4 0"/>
      <parent link="${name}"/>
      <child link="${name}_compensation"/>
   </joint>  
   <link name="${name}_compensation">
      <inertial>
        <mass value="0.789"/>
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <inertia ixx="116e-09" ixy="0" ixz="0" iyy="116e-09" iyz="0" izz="2e-09"/>
      </inertial>
      <!-- <collision name="${name}_part_1_collision"> -->
      <!--   <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!--   <geometry> -->
      <!--     <cylinder length="0.1" radius="0.02"/> -->
      <!--   </geometry> -->
      <!-- </collision> -->
  </link>

  <joint name="${name}_prism_1" type="prismatic">
    <origin rpy="0 0 0" xyz="0.055 -0.065 0"/>
    <child link="${name}_shaft_1"/>
    <parent link="${name}_base"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
    <limit effort="1" lower="0" upper="0.05" velocity="0.1"/>
  </joint>
  <link name="${name}_shaft_1">
      <inertial>
        <mass value="0.063"/>
        <origin rpy="${pi/2} 0 0" xyz="0 0 0 "/>
        <inertia ixx="297e-06" ixy="0" ixz="0" iyy="926e-09" iyz="0" izz="297e-06"/>
      </inertial>
      <!-- <collision name="${name}_shaft_1_collision"> -->
      <!--   <origin rpy="${pi/2} 0 0" xyz="0 0 0 "/> -->
      <!--   <geometry> -->
      <!--     <mesh filename="package://rotors_description/meshes/shaft_wall.dae" scale="1 1 1"/> -->
      <!--   </geometry> -->
      <!-- </collision> -->
      <visual name="${name}_shaft_1_visual">
        <origin rpy="${pi/2} 0 0" xyz="0 0 0 "/>
        <geometry>
          <mesh filename="package://rotors_description/meshes/shaft_wall.dae" scale="1 1 1"/>
        </geometry>
      </visual>
  </link>

  <!-- Connecting base with shaft2 -->
  <joint name="${name}_prism_2" type="prismatic">
    <origin rpy="0 0 0" xyz="-0.055 -0.065 0"/>
    <child link="${name}_shaft_2"/>
    <parent link="${name}_base"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
    <limit effort="1" lower="0" upper="0.05" velocity="0.1"/>
  </joint>
  <link name="${name}_shaft_2">
      <inertial>
        <mass value="0.063"/>
        <origin rpy="${pi/2} 0 0" xyz="0 0 0 "/>
        <inertia ixx="297e-06" ixy="0" ixz="0" iyy="926e-09" iyz="0" izz="297e-06"/>
      </inertial>
      <visual name="${name}_shaft_2_visual">
        <origin rpy="${pi/2} 0 0" xyz="0 0 0 "/>
        <geometry>
          <mesh filename="package://rotors_description/meshes/shaft_wall.dae" scale="1 1 1"/>
        </geometry>
      </visual>
  </link>

  <!-- Connection shaft_wall with plate1 -->
  <joint name="${name}_ball_1_joint_0" type="fixed" >
    <origin xyz="0 -0.12 0" />
    <parent link="${name}_shaft_1" />
    <child link="ball_1_link_1" />
  </joint>
  <joint name="${name}_ball_1_joint_1" type="revolute" >
    <origin xyz="0 0 0.0001" />
    <parent link="ball_1_link_1" />
    <child link="ball_1_link_2" />
    <axis xyz="0 0 1" />
    <dynamics damping="0.005" />
    <limit effort="1" lower="-${ball_joint_limits_h}" upper="${ball_joint_limits_h}" velocity="0.1"/>
  </joint>
  <joint name="${name}_ball_1_joint_2" type="revolute" >
    <origin xyz="0 0 0.0001" />
    <parent link="ball_1_link_2"/>
    <child link="${name}_part_1"/>
    <axis xyz="1 0 0" />
    <dynamics damping="0.005" />
    <limit effort="1" lower="-${ball_joint_limits_v}" upper="${ball_joint_limits_v}" velocity="0.1"/>
  </joint>

  <link name="ball_1_link_1">
      <inertial>
        <mass value="0.0001"/>
        <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
      </inertial>
  </link>

  <link name="ball_1_link_2">
      <inertial>
        <mass value="0.0001"/>
        <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
      </inertial>
  </link>

  <link name="${name}_part_1">
      <inertial>
        <mass value="0.127"/>
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <inertia ixx="608e-06" ixy="36e-09" ixz="-199e-09" iyy="444e-06" iyz="33e-09" izz="994e-06"/>
      </inertial>
      <collision name="${name}_part_1_collision">
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://rotors_description/meshes/half_plate_l_wall.dae" scale="1 1 1"/>
        </geometry>
      </collision>
      <visual name="${name}_part_1_visual">
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://rotors_description/meshes/half_plate_l_wall.dae" scale="1 1 1"/>
        </geometry>
      </visual>
  </link>

  <!-- Connection shaft2 with plate2 -->
  <joint name="${name}_ball_2_joint_0" type="fixed" >
    <origin xyz="0 -0.12 0" />
    <parent link="${name}_shaft_2" />
    <child link="ball_2_link_1" />
  </joint>
  <joint name="${name}_ball_2_joint_1" type="revolute" >
    <origin xyz="0 0 0.0001" />
    <parent link="ball_2_link_1" />
    <child link="ball_2_link_2" />
    <axis xyz="0 0 1" />
    <dynamics damping="0.005" />
    <limit effort="1" lower="-${ball_joint_limits_h}" upper="${ball_joint_limits_h}" velocity="0.1"/>
  </joint>
  <joint name="${name}_ball_2_joint_2" type="revolute" >
    <origin xyz="0 0 0.0001" />
    <parent link="ball_2_link_2"/>
    <child link="${name}_part_2"/>
    <axis xyz="1 0 0" />
    <dynamics damping="0.005" />
    <limit effort="1" lower="-${ball_joint_limits_v}" upper="${ball_joint_limits_v}" velocity="0.1"/>
  </joint>

  <link name="ball_2_link_1">
      <inertial>
        <mass value="0.0001"/>
        <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
      </inertial>
  </link>

  <link name="ball_2_link_2">
      <inertial>
        <mass value="0.0001"/>
        <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
      </inertial>
  </link>

  <link name="${name}_part_2">
      <inertial>
        <mass value="0.127"/>
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <inertia ixx="608e-06" ixy="36e-09" ixz="-199e-09" iyy="444e-06" iyz="33e-09" izz="994e-06"/>
      </inertial>
      <collision name="${name}_part_2_collision">
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://rotors_description/meshes/half_plate_r_wall.dae" scale="1 1 1"/>
        </geometry>
      </collision>
      <visual name="${name}_part_2_visual">
        <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="package://rotors_description/meshes/half_plate_r_wall.dae" scale="1 1 1"/>
        </geometry>
      </visual>
  </link>

  <joint name="${name}_sensor_1" type="fixed" >
    <origin xyz="0.075 -0.01 0"  rpy="0 0 -${pi/2}"/>
    <parent link="${name}_base" />
    <child link="sensor_1_link" />
  </joint>
  <link name="sensor_1_link">
    <inertial>
      <mass value="0.0002"/>
      <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
    </inertial>
  </link>

  <joint name="${name}_sensor_2" type="fixed" >
    <origin xyz="-0.075 -0.01 0"  rpy="0 0 -${pi/2}"/>
    <parent link="${name}_base" />
    <child link="sensor_2_link" />
  </joint>
  <link name="sensor_2_link">
    <inertial>
      <mass value="0.0002"/>
      <inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
    </inertial>
  </link>

  <gazebo>
    <joint name="joint_connecting_plates" type="fixed">
      <pose>0.055 0 0 0 0 0</pose>
      <parent>${name}_part_1</parent>
      <child>${name}_part_2</child>
    </joint>
  </gazebo>

  <gazebo reference="${name}_prism_1">
    <implicitSpringDamper>true</implicitSpringDamper>
    <springStiffness>${stiffness}</springStiffness>
    <springReference>0</springReference>
  </gazebo>

  <gazebo reference="${name}_prism_2">
    <implicitSpringDamper>true</implicitSpringDamper>
    <springStiffness>${stiffness}</springStiffness>
    <springReference>0</springReference>
  </gazebo>

  <gazebo reference="sensor_1_link">
        <sensor name="sensor_1" type='ray'>
          <update_rate>100</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>10</samples>
                <resolution>5</resolution>
                <min_angle>-0.01</min_angle>
                <max_angle>0.01</max_angle>
              </horizontal>
              <vertical>
                <samples>10</samples>
                <resolution>5</resolution>
                <min_angle>-0.01</min_angle>
                <max_angle>0.01</max_angle>
              </vertical>
            </scan>
            <range>
              <min>0.002</min>
              <max>0.5</max>
              <resolution>0.01</resolution>
            </range>
          </ray>
          <plugin name='gazebo_ros_range' filename='libgazebo_ros_range.so'>
            <gaussianNoise>0.001</gaussianNoise>
            <alwaysOn>true</alwaysOn>
            <updateRate>100</updateRate>
            <topicName>wall/sensor1</topicName>
            <frameName>001</frameName>
            <fov>0.01</fov>
            <radiation>radiation</radiation>
          </plugin>
        </sensor>
      </gazebo>

     <gazebo reference="sensor_2_link">
        <sensor name="sensor_2" type='ray'>
          <update_rate>100</update_rate>
          <ray>
            <scan>
              <horizontal>
                <samples>10</samples>
                <resolution>5</resolution>
                <min_angle>-0.01</min_angle>
                <max_angle>0.01</max_angle>
              </horizontal>
              <vertical>
                <samples>10</samples>
                <resolution>5</resolution>
                <min_angle>-0.01</min_angle>
                <max_angle>0.01</max_angle>
              </vertical>
            </scan>
            <range>
              <min>0.002</min>
              <max>0.5</max>
              <resolution>0.01</resolution>
            </range>
          </ray>
          <plugin name='gazebo_ros_range' filename='libgazebo_ros_range.so'>
            <gaussianNoise>0.001</gaussianNoise>
            <alwaysOn>true</alwaysOn>
            <updateRate>100</updateRate>
            <topicName>wall/sensor2</topicName>
            <frameName>002</frameName>
            <fov>0.01</fov>
            <radiation>radiation</radiation>
          </plugin>
        </sensor>
      </gazebo>
  </xacro:macro>

</robot>
