#!/bin/sh
# shellcheck disable=SC2154

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

# For simulation, allow registering modes while armed for developer convenience
param set-default COM_MODE_ARM_CHK 1

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

	echo "INFO  [init] SIH simulator"

	if [ -n "${PX4_HOME_LAT}" ]; then
		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
	fi

	if [ -n "${PX4_HOME_LON}" ]; then
		param set SIH_LOC_LON0 ${PX4_HOME_LON}
	fi
	if [ -n "${PX4_HOME_ALT}" ]; then
		param set SIH_LOC_H0 ${PX4_HOME_ALT}
	fi

	if simulator_sih start; then

		if param compare -s SENS_EN_BAROSIM 1
		then
			sensor_baro_sim start
		fi
		if param compare -s SENS_EN_GPSSIM 1
		then
			sensor_gps_sim start
		fi
		if param compare -s SENS_EN_MAGSIM 1
		then
			sensor_mag_sim start
		fi
		if param compare -s SENS_EN_AGPSIM 1
		then
			sensor_agp_sim start
		fi

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

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

	echo "INFO  [init] Gazebo simulator"

	if [ -z "${PX4_GZ_STANDALONE}" ]; then

		# Enforce minimum gz version as Harmonic (gz-sim8)
		MIN_GZ_VERSION="8.0.0"
		GZ_SIM_VERSION=$(gz sim --versions 2>/dev/null | head -n 1 | tr -d ' ')

		if [ -z "$GZ_SIM_VERSION" ]; then
		    echo "ERROR [init] Gazebo gz sim not found. Please install gz-harmonic"
		    exit 1
		fi

		# Use sort compare, check that MIN_GZ_VERSION is ordered last
		if [ "$(printf '%s\n' "$GZ_SIM_VERSION" "$MIN_GZ_VERSION" | sort -V | head -n1)" = "$MIN_GZ_VERSION" ]; then
			gz_command="gz"
			gz_sub_command="sim"

			# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
			# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
			if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
				echo "INFO  [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!"
				gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
			fi
		else
		    echo "ERROR [init] Gazebo gz sim version is too old ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
		    exit 1
		fi

		# Look for an already running 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_WORLD}" ]; then

			# Setup gz environment variables
			if [ -f ./gz_env.sh ]; then
				. ./gz_env.sh

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

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

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

			if [ -z "${HEADLESS}" ]; then
				echo "INFO  [init] Starting gz gui"
				${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
			fi

			# Wait for Gazebo world to be ready before proceeding
			check_scene_info() {
				SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1)
				if echo "$SERVICE_INFO" | grep -q "Service providers"; then
					return 0
				else
					return 1
				fi
			}

			ATTEMPTS=30
			while [ $ATTEMPTS -gt 0 ]; do
				if check_scene_info; then
					echo "INFO  [init] Gazebo world is ready"
					break
				fi
				ATTEMPTS=$((ATTEMPTS-1))
				if [ $ATTEMPTS -eq 0 ]; then
					echo "ERROR [init] Timed out waiting for Gazebo world"
					exit 1
				fi
				echo "INFO  [init] Waiting for Gazebo world..."
				sleep 1
			done

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

	else
		echo "INFO  [init] Standalone PX4 launch, waiting for Gazebo"
	fi

	# Start gz_bridge - either spawn a model or connect to existing one
	if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
		# Spawn a model
		MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
		MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"

		POSE_ARG=""
		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
			pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
			pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
			pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
			pos_x=${pos_x:-0}
			pos_y=${pos_y:-0}
			pos_z=${pos_z:-0}

			POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }"
			echo "INFO  [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
		fi

		echo "INFO  [init] Spawning model"

		# Spawn model
		${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
				--reptype gz.msgs.Boolean --timeout 5000 \
				--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1

		# Wait for model to spawn
		sleep 1

		# Start gz_bridge
		if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then
			echo "ERROR [init] gz_bridge failed to start and spawn model"
			exit 1
		fi

		# Set physics parameters for faster-than-realtime simulation if needed
		if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then
			echo "INFO  [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}"
			${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \
						--reptype gz.msgs.Boolean --timeout 5000 \
						--req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1
		fi

		# Set up camera to follow the model if requested
		if [ -n "${PX4_GZ_FOLLOW}" ]; then

			echo "INFO  [init] Setting camera to follow ${MODEL_NAME_INSTANCE}"

			# Set camera to follow the model
			${gz_command} service -s "/gui/follow" --reqtype gz.msgs.StringMsg \
					--reptype gz.msgs.Boolean --timeout 5000 \
					--req "data: \"${MODEL_NAME_INSTANCE}\"" > /dev/null 2>&1

			# Set default camera offset if not specified
			follow_x=${PX4_GZ_FOLLOW_OFFSET_X:--2.0}
			follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:--2.0}
			follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:-2.0}

			# Set camera offset
			${gz_command} service -s "/gui/follow/offset" --reqtype gz.msgs.Vector3d \
					--reptype gz.msgs.Boolean --timeout 5000 \
					--req "x: ${follow_x}, y: ${follow_y}, z: ${follow_z}" > /dev/null 2>&1

			echo "INFO  [init] Camera follow offset set to ${follow_x}, ${follow_y}, ${follow_z}"
		fi

	elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
		# Connect to existing model
		echo "INFO  [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model"
		if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${PX4_GZ_MODEL_NAME}"; then
			echo "ERROR [init] gz_bridge failed to start and attach to existing model"
			exit 1
		fi
	else
		echo "ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
		exit 1
	fi
	# Start the sensor simulator modules
	if param compare -s SENS_EN_BAROSIM 1
	then
		sensor_baro_sim start
	fi
	if param compare -s SENS_EN_GPSSIM 1
	then
		sensor_gps_sim start
	fi
	if param compare -s SENS_EN_MAGSIM 1
	then
		sensor_mag_sim start
	fi
	if param compare -s SENS_EN_ARSPDSIM 1
	then
		sensor_airspeed_sim start
	fi
	if param compare -s SENS_EN_AGPSIM 1
	then
		sensor_agp_sim start
	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

	# EKF2 specifics
	param set-default EKF2_GPS_DELAY 10
	param set-default EKF2_MULTI_IMU 3
	param set-default SENS_IMU_MODE 0

	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