#!/bin/sh
# shellcheck disable=SC2154

# Simulator IMU data provided at 250 Hz
param set-default IMU_INTEG_RATE 250

if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then

	echo "INFO  [init] SIH simulator"

	if simulator_sih start; then

		sensor_baro_sim start
		sensor_gps_sim start
		sensor_mag_sim start

	else
		echo "ERROR [init] simulator_sih failed to start"
		exit 1
	fi

elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then

	# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
	if [ -f ./gz_env.sh ]; then
		. ./gz_env.sh

	elif [ -f ../gz_env.sh ]; then
		. ../gz_env.sh
	fi

	# "gz sim" only avaiilable in Garden and later
	GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
	if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
	then
		# "gz sim" from Garden on
		gz_command="gz"
		gz_sub_command="sim"
	else
		echo "ERROR [init] Gazebo gz please install gz-garden"
		exit 1
	fi

	# look for running ${gz_command} gazebo world
	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )

	# shellcheck disable=SC2153
	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then

		echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"

		${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &

		if [ -z "${HEADLESS}" ]; then
			# HEADLESS not set, starting gui
			${gz_command} ${gz_sub_command} -g &
		fi

	else
		echo "INFO  [init] gazebo already running world: ${gz_world}"
		PX4_GZ_WORLD=${gz_world}
	fi

	# start gz_bridge
	if [ -n "${PX4_GZ_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
		# model specified, gz_bridge will spawn model

		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
			# Clean potential input line formatting.
			model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/  / /g; s/ /,/g' )"
			echo "INFO  [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}"

		else
			echo "WARN  [init] PX4_GZ_MODEL_POSE not set, spawning at origin."
			model_pose="0,0,0,0,0,0"
		fi

		# start gz bridge with pose arg.
		if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
			sensor_baro_sim start
			sensor_gps_sim start
			sensor_mag_sim start
			if param compare -s SENS_EN_ARSPDSIM 1
			then
				sensor_airspeed_sim start
			fi

		else
			echo "ERROR [init] gz_bridge failed to start"
			exit 1
		fi

	elif [ -n "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then
		# model name specificed, gz_bridge will attach to existing model

		if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
			sensor_baro_sim start
			sensor_gps_sim start
			sensor_mag_sim start
			if param compare -s SENS_EN_ARSPDSIM 1
			then
				sensor_airspeed_sim start
			fi

		else
			echo "ERROR [init] gz_bridge failed to start"
			exit 1
		fi

	elif [ -n "${PX4_SIM_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then

		echo "WARN  [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL."

		if gz_bridge start -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
			sensor_baro_sim start
			sensor_gps_sim start
			sensor_mag_sim start
			if param compare -s SENS_EN_ARSPDSIM 1
			then
				sensor_airspeed_sim start
			fi

		else
			echo "ERROR [init] gz_bridge failed to start"
			exit 1
		fi

	else
		echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_GZ_MODEL"
		exit 1
	fi

elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then

	echo "INFO  [init] jMAVSim simulator"

	if jps | grep -i jmavsim; then
		kill "$(jps | grep -i jmavsim | awk '{print $1}')" || true
		sleep 1
	fi

	param set IMU_INTEG_RATE 250
	./jmavsim_run.sh -l -r 250 &

	simulator_mavlink start -h localhost $((4560+px4_instance))

else
	# otherwise start simulator (mavlink) module
	simulator_tcp_port=$((4560+px4_instance))

	# Check if PX4_SIM_HOSTNAME environment variable is empty
	# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
	# If both are empty use localhost for simulator
	if [ -z "${PX4_SIM_HOSTNAME}" ]; then

		if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
			echo "INFO  [init] PX4_SIM_HOSTNAME: localhost"
			simulator_mavlink start -c $simulator_tcp_port
		else
			echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}"
			simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}"
		fi

	else
		echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
		simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
	fi

fi