#!/bin/bash
set -e #PR #6
source /opt/ros/$ROS_DISTRO/setup.bash

if [[ -z $SIM_DRONE_COUNT ]]
then
  echo "SIM_DRONE_COUNT environment variable is not set."
  exit 1
fi

gz sim -v -r $WORLD_FILE_NAME &
while true
do
  topics=$(gz topic -l)
  if [[ $topics == *"/world/$WORLD_NAME"* ]]
  then
    break
  fi
  sleep 1
done

cd /ardupilot_gazebo/models/$DRONE_MODEL
for (( j=0; j<$SIM_DRONE_COUNT; j++ ));
do
    xacro -v gstreamer_udp_port:=$(($GSTREAMER_UDP_PORT + ($j * 10))) fdm_port_in:=$(($FDM_PORT_IN + ($j * 10))) model.xacro.sdf -o model.sdf
    #! string is better than using -file option. File is not up to date in the next iteration.
    value=$(</ardupilot_gazebo/models/$DRONE_MODEL/model.sdf)
    gz_drone_name=spiri-$(($j + 1))
    #? Maybe read from text file for formation of drones? x y z r p y
    ros2 run ros_gz_sim create -world $WORLD_NAME -string "$value" -name $gz_drone_name -x $j  -y 0 -z 0.195
done
exec "$@"