<?xml version="1.0" ?>
<%
  # Tile world generator
  #
  # Command line options (usage erb [opt]=[arg]):
  # s: seed for randomization, defaults to random
  # t: tile type, defaults to 'tunnel'
  # n_x: number of tiles on X dimension, defaults to 20
  # n_y: number of tiles on Y dimension, must be odd, defaults to 5

  ###############################################
  #                                             #
  #           COMMAND LINE ARGUMENTS            #
  #                                             #
  ###############################################

  vars = ARGV.take_while {|arg| arg[/^\w+=/]}
  ARGV.slice!(0, vars.size)
  vars.each do |var|
    k, v = var.split('=', 2)
    TOPLEVEL_BINDING.eval %Q(#{k} = "#{v}")
  end

  # Seed
  seed = rand(0..10000000)
  if (defined? s)
    seed = s.to_i
  end
  srand(seed)

  # Tile type
  $type = 'tunnel'
  if (defined? t)
    $type = t.to_s
    if ($type != 'cave') && ($type != 'tunnel')
      raise('Unsupported tile type')
      exit
    end
  end

  # Number of tiles per dimension
  $n_x = 20
  if (defined? n_x)
    $n_x = n_x.to_i
  end
  $n_y = 5
  if (defined? n_y)
    $n_y = n_y.to_i
  end

  ###############################################
  #                                             #
  #                TILE PARAMS                  #
  #                                             #
  ###############################################

  # Tile X/Y dimensions in meters
  tile_dimension = 20

  # Entrance offset from staging area on X axis
  entrance_offset = tile_dimension

  # Increase this number as more tile meshes are added
  type_total_count = 0
  if ($type == 'cave')
    type_total_count = 10
  elsif ($type == 'tunnel')
    type_total_count = 1
  end

  # Keep all possible tile types
  tile_types = []
  for i in (1..type_total_count)
    tile_types.push($type + '_tile_' + i.to_s)
  end

  ###############################################
  #                                             #
  #               RANDOMIZATION                 #
  #                                             #
  ###############################################

  # Get -1 if n < 0 and +1 if n >= 0
  def sign(_n)
    return _n == 0 ? 1 : _n.abs / _n
  end

  shapesCollection = {
    "tunnel_tile_1" => [
      {
        :pose => "<pose>0 0 -0.25 0 0 0</pose>",
        :geometry => "<box><size>5 20 0.5</size></box>"
      },
      {
        :pose => "<pose>0 0 -0.25 0 0 1.57</pose>",
        :geometry => "<box><size>5 20 0.5</size></box>"
      },
      {
        :pose => "<pose>-2.5 6.25 2 3.14 -1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>2.5 6.25 2 3.14 -1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>-2.5 -6.25 2 3.14 -1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>2.5 -6.25 2 3.14 -1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>6.25 2.5 2 1.57 1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>-6.25 2.5 2 1.57 1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>6.25 -2.5 2 1.57 1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>-6.25 -2.5 2 1.57 1.57 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
    ],
    "tunnel_tile_2" => [
      {
        :pose => "<pose>4 0 -0.25 0 0 0</pose>",
        :geometry => "<box><size>5 7.5 0.05</size></box>"
      },
      {
        :pose => "<pose>6.25 2.5 -0.25 0 0 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
      {
        :pose => "<pose>7.5 2.5 4.25 0 0 0</pose>",
        :geometry => "<box><size>5 7.5 0.5</size></box>"
      },
    ],
    "tunnel_tile_3" => [
      {
        :pose => "<pose>0 0 0 0 0 0</pose>",
        :geometry => "<box><size>10 10 1</size></box>"
      }
    ],
    "tunnel_tile_4" => [
      {
        :pose => "<pose>0 0 0 0 0 0</pose>",
        :geometry => "<box><size>10 10 1</size></box>"
      }
    ],
    "tunnel_tile_5" => [
      {
        :pose => "<pose>0 0 0 0 0 0</pose>",
        :geometry => "<box><size>10 10 1</size></box>"
      }
    ],
    "tunnel_tile_6" => [
      {
        :pose => "<pose>0 0 0 0 0 0</pose>",
        :geometry => "<box><size>10 10 1</size></box>"
      }
    ],
    "tunnel_tile_7" => [
      {
        :pose => "<pose>0 0 0 0 0 0</pose>",
        :geometry => "<box><size>10 10 1</size></box>"
      }
    ]
  }
  # Grid tiles
  half_n_y = ($n_y/2).floor
  tiles = []
  tile_count = 0
  for x in (0..($n_x - 1))
    for y in (-half_n_y..half_n_y)
      tile_count += 1
      tile =
      {
        :name => "tile_#{tile_count}",
        :type => tile_types.sample(),
        :x => entrance_offset + tile_dimension * x,
        :y => tile_dimension * y,
        :yaw => [0, Math::PI*0.5, -Math::PI*0.5, Math::PI].sample()
      }
      tiles.push(tile)
    end
  end

  # Create vehilce at given pose
  vehicles =
  [
    {
      :name => "vehicle_red",
      :pose => "0 0 0.325 0 0 0",
      :color => "1.0 0.5 0.5 1"
    },
    {
      :name => "vehicle_blue",
      :pose => "0 20 0.325 0 0 0",
      :color => "0.5 0.5 1.0 1"
    },
  ]
%>
<!--
  Generated from tile.world.erb
  Seed: <%= seed %>
  Type: <%= $type %>
  N_x: <%= $n_x %>
  N_y: <%= $n_y %>
-->
<sdf version="1.6">
  <world name="default">
    <plugin
     filename="gz-sim-physics-system"
     name="gz::sim::systems::Physics">
    </plugin>
    <plugin
      filename="gz-sim-scene-broadcaster-system"
      name="gz::sim::systems::SceneBroadcaster">
    </plugin>

    <scene>
      <ambient>0.8 0.8 0.8 1.0</ambient>
      <background>0.34 0.39 0.43 1.0</background>
      <grid>false</grid>
      <origin_visual>false</origin_visual>
    </scene>

    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="stage">
      <pose>-12 0 -5 0 0 0</pose>
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>50 100 10</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>50 100 10</size>
            </box>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <!-- Grid tiles -->
  <%
    tiles.each_with_index do |tile, count|
  %>
    <model name="<%= tile[:name] %>">
      <pose><%= tile[:x] %> <%= tile[:y] %> 0 0 0 <%= tile[:yaw] %></pose>
      <static>true</static>
      <link name="link">
      <% shapesCollection[tile[:type]].each_with_index do |shape, index| %>
        <collision name="collision_<%= index %>">
          <%= shape[:pose] %>
          <geometry>
            <%= shape[:geometry] %>
          </geometry>
        </collision>
        <visual name="visual_<%= index %>">
          <%= shape[:pose] %>
          <geometry>
            <%= shape[:geometry] %>
          </geometry>
          <material>
            <ambient>0 1 0 0.8</ambient>
            <diffuse>0 1 0 0.8</diffuse>
            <specular>1 1 1 0.8</specular>
          </material>
        </visual>
      <%
        end
      %>
      </link>
    </model>
  <%
    end
  %>

  <% vehicles.each do |vehicle| %>
  <model name='<%= vehicle[:name] %>'>
    <pose><%= vehicle[:pose] %></pose>
    <link name='chassis'>
      <pose>-0.151427 -0 0.175 0 -0 0</pose>
      <inertial>
        <mass>1.14395</mass>
        <inertia>
          <ixx>0.126164</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.416519</iyy>
          <iyz>0</iyz>
          <izz>0.481014</izz>
        </inertia>
      </inertial>
      <visual name='visual'>
        <geometry>
          <box>
            <size>2.01142 1 0.568726</size>
          </box>
        </geometry>
        <material>
          <ambient><%= vehicle[:color] %></ambient>
          <diffuse><%= vehicle[:color] %></diffuse>
          <specular><%= vehicle[:color] %></specular>
        </material>
      </visual>
      <collision name='collision'>
        <geometry>
          <box>
            <size>2.01142 1 0.568726</size>
          </box>
        </geometry>
      </collision>
    </link>

    <link name='left_wheel'>
      <pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
      <inertial>
        <mass>2</mass>
        <inertia>
          <ixx>0.145833</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.145833</iyy>
          <iyz>0</iyz>
          <izz>0.125</izz>
        </inertia>
      </inertial>
      <visual name='visual'>
        <geometry>
          <sphere>
            <radius>0.3</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0.2 0.2 0.2 1</ambient>
          <diffuse>0.2 0.2 0.2 1</diffuse>
          <specular>0.2 0.2 0.2 1</specular>
        </material>
      </visual>
      <collision name='collision'>
        <geometry>
          <sphere>
            <radius>0.3</radius>
          </sphere>
        </geometry>
      </collision>
    </link>

    <link name='right_wheel'>
      <pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
      <inertial>
        <mass>2</mass>
        <inertia>
          <ixx>0.145833</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.145833</iyy>
          <iyz>0</iyz>
          <izz>0.125</izz>
        </inertia>
      </inertial>
      <visual name='visual'>
        <geometry>
          <sphere>
            <radius>0.3</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0.2 0.2 0.2 1</ambient>
          <diffuse>0.2 0.2 0.2 1</diffuse>
          <specular>0.2 0.2 0.2 1</specular>
        </material>
      </visual>
      <collision name='collision'>
        <geometry>
          <sphere>
            <radius>0.3</radius>
          </sphere>
        </geometry>
      </collision>
    </link>

    <link name='caster'>
      <pose>-0.957138 -0 -0.125 0 -0 0</pose>
      <inertial>
        <mass>1</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <visual name='visual'>
        <geometry>
          <sphere>
            <radius>0.2</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0.2 0.2 0.2 1</ambient>
          <diffuse>0.2 0.2 0.2 1</diffuse>
          <specular>0.2 0.2 0.2 1</specular>
        </material>
      </visual>
      <collision name='collision'>
        <geometry>
          <sphere>
            <radius>0.2</radius>
          </sphere>
        </geometry>
      </collision>
    </link>

    <joint name='left_wheel_joint' type='revolute'>
      <parent>chassis</parent>
      <child>left_wheel</child>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
        </limit>
      </axis>
    </joint>

    <joint name='right_wheel_joint' type='revolute'>
      <parent>chassis</parent>
      <child>right_wheel</child>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1.79769e+308</lower>
          <upper>1.79769e+308</upper>
        </limit>
      </axis>
    </joint>

    <joint name='caster_wheel' type='ball'>
      <parent>chassis</parent>
      <child>caster</child>
    </joint>

    <plugin
      filename="gz-sim-diff-drive-system"
      name="gz::sim::systems::DiffDrive">
      <left_joint>left_wheel_joint</left_joint>
      <right_joint>right_wheel_joint</right_joint>
      <wheel_separation>1.25</wheel_separation>
      <wheel_radius>0.3</wheel_radius>
    </plugin>

  </model>
  <% end %>

  <plugin name="gz::sim" filename="dummy">
  <% vehicles.each do |vehicle| %>
    <performer name="perf_<%= vehicle[:name] %>">
      <ref><%= vehicle[:name] %></ref>
      <geometry>
        <box>
          <size>2 2 2</size>
        </box>
      </geometry>
    </performer>
  <% end %>

    <%
      tiles.each_with_index do |tile, count|
      next if count == $n_y/2
    %>
    <level name="level<%= count.to_s %>">
      <pose><%= tile[:x] %> <%= tile[:y] %> 2.5 0 0 0</pose>
      <geometry>
        <box>
          <size>40 40 1000</size>
        </box>
      </geometry>
      <ref><%= tile[:name] %></ref>
    </level>
    <%
      end
    %>

    <%
      tiles.each_slice(half_n_y*2).with_index do |(*slice), count|
        xavg = slice.map{|tile| tile[:x]}.reduce(:+)/slice.size
        yavg = slice.map{|tile| tile[:y]}.reduce(:+)/slice.size

        # comment this out to have extra levels
        next

    %>
    <level name="levelA_<%= count.to_s %>">
      <pose><%= xavg %> <%= yavg %> 2.5 0 0 0</pose>
      <geometry>
        <box>
        <size>40 <%= slice.size*40 %> 1000</size>
        </box>
      </geometry>
      <% slice.each do |tile| %>
      <ref><%= tile[:name] %></ref>
      <% end %>
    </level>
    <%
      end
    %>
  </plugin>

  </world>
</sdf>
