<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find lite3_description)/xacro/transmission.xacro"/>

  <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">

    <joint name="${name}_HipX" type="revolute">
      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
        <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
      </xacro:if>
      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
        <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
      </xacro:if>
      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
      </xacro:if>
      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
      </xacro:if>
      <parent link="TORSO"/>
      <child link="${name}_HIP"/>
      <axis xyz="-1 0 0"/>
      <dynamics damping="${damping}" friction="${friction}"/>
      <limit lower="-0.42" upper="0.42" effort="24" velocity="26"/>
    </joint>

    <link name="${name}_HIP">
      <inertial>
        <inertia ixx="0.00014538" ixy="8.1579E-07" ixz="-1.264E-05" iyy="0.00024024" iyz="1.3443E-06" izz="0.00013038"/>
        <origin xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
        <mass value="${hip_mass}"/>
        <inertia
            ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
            iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
            izz="${hip_izz}"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="${-0.05*front_hind} 0.0 0.0"/>
        <geometry>
          <mesh filename="file://$(find lite3_description)/meshes/${name}_HIP.dae"/>
        </geometry>
      </visual>
    </link>

    <joint name="${name}_HipY" type="revolute">
      <origin xyz="0 ${thigh_offset*mirror} 0"/>
      <parent link="${name}_HIP"/>
      <child link="${name}_THIGH"/>
      <axis xyz="0 -1 0"/>
      <dynamics damping="${damping}" friction="${friction}"/>
      <limit lower="-2.67" upper="0.314" effort="24" velocity="26"/>
    </joint>

    <link name="${name}_THIGH">
      <inertial>
        <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
        <mass value="${thigh_mass}"/>
        <inertia
            ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
            iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
            izz="${thigh_izz}"/>
        <inertia ixx="0.001" ixy="-2.5E-06" ixz="-1.12E-04" iyy="0.00116" iyz="3.75E-07" izz="2.68E-04"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
        <geometry>
          <xacro:if value="${mirror_dae == True}">
            <mesh filename="file://$(find lite3_description)/meshes/l_thigh.dae"/>
          </xacro:if>
          <xacro:if value="${mirror_dae == False}">
            <mesh filename="file://$(find lite3_description)/meshes/r_thigh.dae"/>
          </xacro:if>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 -0.08"/>
        <geometry>
          <box size="0.02 0.02 0.16"/>
        </geometry>
      </collision>
    </link>

    <joint name="${name}_Knee" type="revolute">
      <origin xyz="0 0 -0.20"/>
      <parent link="${name}_THIGH"/>
      <child link="${name}_SHANK"/>
      <axis xyz="0 -1 0"/>
      <dynamics damping="${damping}" friction="${friction}"/>
      <limit lower="0.6" upper="2.72" effort="36" velocity="17"/>
    </joint>

    <link name="${name}_SHANK">
      <inertial>
        <origin xyz="0.00585 -8.732E-07 -0.12"/>
        <mass value="0.115"/>
        <inertia ixx="6.68E-04" ixy="-1.24E-08" ixz="6.91E-06" iyy="6.86E-04" iyz="5.65E-09" izz="3.155E-05"/>
      </inertial>
      <visual>
        <origin rpy="3.14 -1.42 0" xyz="0.0 0.0 0.0"/>
        <geometry>
          <mesh filename="file://$(find lite3_description)/meshes/SHANK.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 -0.09"/>
        <geometry>
          <box size="0.02 0.02 0.18"/>
        </geometry>
      </collision>
    </link>

    <joint name="${name}_Ankle" type="fixed">
      <origin xyz="0 0 -0.21"/>
      <parent link="${name}_SHANK"/>
      <child link="${name}_FOOT"/>
    </joint>

    <link name="${name}_FOOT">
      <inertial>
        <mass value="${foot_mass}"/>
        <inertia
            ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
            iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
            izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
      </inertial>
      <collision>
        <geometry>
          <sphere radius="${foot_radius}"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${name}_HIP">
      <mu1>0.2</mu1>
      <mu2>0.2</mu2>
    </gazebo>

    <gazebo reference="${name}_THIGH">
      <mu1>0.2</mu1>
      <mu2>0.2</mu2>
      <self_collide>0</self_collide>
      <kp value="1000000.0"/>
      <kd value="100.0"/>
    </gazebo>

    <gazebo reference="${name}_SHANK">
      <mu1>0.6</mu1>
      <mu2>0.6</mu2>
      <self_collide>1</self_collide>
    </gazebo>

    <gazebo reference="${name}_FOOT">
      <mu1>0.6</mu1>
      <mu2>0.6</mu2>
      <self_collide>1</self_collide>
      <kp value="1000000.0"/>
      <kd value="100.0"/>
    </gazebo>

    <xacro:leg_transmission name="${name}"/>

  </xacro:macro>

</robot>
