#!/bin/sh

# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
set +e
# Un comment the line below to help debug scripts by printing a trace of the script commands
#set -x

# PX4 commands need the 'px4-' prefix in bash.
# (px4-alias.sh is expected to be in the PATH)
# shellcheck disable=SC1091
. px4-alias.sh

#search path for sourcing px4-rc.*
PATH="$PATH:${R}etc/init.d-posix"

#
# Main SITL startup script
#

# check for ekf2 replay
# shellcheck disable=SC2154
if [ "$replay_mode" = "ekf2" ]
then
	. ${R}etc/init.d-posix/rc.replay
	exit 0
fi

# initialize script variables
set VEHICLE_TYPE none
set LOGGER_ARGS ""
set LOGGER_BUF 1000

set RUN_MINIMAL_SHELL no

set SYS_AUTOSTART=0

if [ "$PX4_SIM_MODEL" = "shell" ]
then
	set RUN_MINIMAL_SHELL yes

elif [ -n "$PX4_SYS_AUTOSTART" ]
then
	echo "env SYS_AUTOSTART: ${PX4_SYS_AUTOSTART}"
	SYS_AUTOSTART=${PX4_SYS_AUTOSTART}

elif [ "$PX4_SIM_MODEL" = "none" ] || [ -z $PX4_SIM_MODEL ]
then
	# no airframe selected
	SYS_AUTOSTART=0

elif [ -n "$PX4_SIM_MODEL" ]
then
	# Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL})
	# TODO: unify with rc.autostart generation
	# shellcheck disable=SC2012
	REQUESTED_AUTOSTART=$(ls "${R}etc/init.d-posix/airframes" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p')
	if [ -z "$REQUESTED_AUTOSTART" ]; then
		echo "ERROR [init] Unknown model $PX4_SIM_MODEL (not found by name on ${R}etc/init.d-posix/airframes)"
		exit 1
	else
		SYS_AUTOSTART=$REQUESTED_AUTOSTART
		echo "INFO  [init] found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART"
	fi
fi

# Load parameters
set PARAM_FILE fs/parameters.bson
set PARAM_BACKUP_FILE fs/parameters_backup.bson

param select $PARAM_FILE
if [ -f $PARAM_FILE ]; then

	if ! param import
	then
		echo "ERROR [init] param import failed"

		bsondump $PARAM_FILE

		# try to make a backup copy
		cp $PARAM_FILE param_import_fail.bson

		# try importing from backup file
		if [ -f $PARAM_BACKUP_FILE ]
		then
			echo "[init] importing from parameter backup"

			# dump current backup file contents for comparison
			bsondump $PARAM_BACKUP_FILE

			param import $PARAM_BACKUP_FILE

			# overwrite invalid $PARAM_FILE with backup
			cp $PARAM_BACKUP_FILE $PARAM_FILE
		fi

		param status
	fi

elif [ -f $PARAM_BACKUP_FILE ]; then
	echo "ERROR [init] primary param file $PARAM_FILE unavailable, using backup $PARAM_BACKUP_FILE"
	param import $PARAM_BACKUP_FILE
fi

param select-backup $PARAM_BACKUP_FILE


# exit early when the minimal shell is requested
[ $RUN_MINIMAL_SHELL = yes ] && exit 0

if param compare SYS_AUTOSTART $SYS_AUTOSTART
then
	set AUTOCNF no

elif [ "$SYS_AUTOSTART" -eq 0 ]
then
	set AUTOCNF no
else
	set AUTOCNF yes
	param set SYS_AUTOCONFIG 1
fi

# To trigger a parameter reset during boot SYS_AUTOCONFIG was set to 1 before
set PARAM_RESET_DONE no
if param greater SYS_AUTOCONFIG 0
then
	# Reset parameters except airframe, parameter version, sensor calibration, total flight time, flight UUID, MAVLink sys_id
	param reset_all SYS_AUTOSTART SYS_PARAM_VER SYS_AUTOCFG_CAL CAL_* LND_FLIGHT* TC_* COM_FLIGHT* MAV_SYS_ID
	set AUTOCNF yes
	set PARAM_RESET_DONE yes
fi

# multi-instance setup
# shellcheck disable=SC2154
param set MAV_SYS_ID $((px4_instance+1))
param set UXRCE_DDS_KEY $((px4_instance+1))

if [ $AUTOCNF = yes ]
then
	param set SYS_AUTOSTART $SYS_AUTOSTART
fi

# Simulated sensor calibration, declared via SYS_AUTOCFG_CAL the same way a real airframe's built-in calibration would
param set-default SYS_AUTOCFG_CAL 1

param set-default CAL_ACC0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
param set-default CAL_GYRO0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
param set-default CAL_ACC1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
param set-default CAL_GYRO1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
param set-default CAL_ACC2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
param set-default CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION

param set-default CAL_MAG0_ID 197388
param set-default CAL_MAG0_PRIO 50
param set-default CAL_MAG1_ID 197644
param set-default CAL_MAG1_PRIO 50

param set-default SENS_BOARD_X_OFF 0.000001
param set-default SENS_DPRES_OFF 0.001

param set-default BAT1_N_CELLS 4

# disable check, no CPU load reported on posix yet
param set-default COM_CPU_MAX -1
param set-default COM_RAM_MAX -1

# Don't require RC calibration and configuration
param set-default COM_RC_IN_MODE 1

# Speedup SITL startup
param set-default EKF2_GPS_YAW_RST 0.5
param set-default GNSS_REQ_TIME 0.5

param set-default IMU_GYRO_FFT_EN 1

param set-default -s MC_AT_EN 1

# By default log from boot until first disarm.
param set-default SDLOG_MODE 1
# enable default, estimator replay and vision/avoidance logging profiles
param set-default SDLOG_PROFILE 131
param set-default SDLOG_DIRS_MAX 7

param set-default TRIG_INTERFACE 3

param set-default SYS_FAILURE_EN 1
# Enable low-battery actions by default for (automated) testing. Battery sim
# does not go below 50% by default, but failure injection can trigger failsafes.
param set-default COM_LOW_BAT_ACT 3

# set default IP to localhost
param set-default UXRCE_DDS_AG_IP 2130706433 # 127.0.0.1


# Adapt timeout parameters if simulation runs faster or slower than realtime.
# Uses awk for the float multiplication; bc is not installed in all CI containers.
if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
	COM_DL_LOSS_T_LONGER=$(awk -v f="$PX4_SIM_SPEED_FACTOR" 'BEGIN{print f * 10}')
	echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
	param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER

	COM_RC_LOSS_T_LONGER=$(awk -v f="$PX4_SIM_SPEED_FACTOR" 'BEGIN{print f * 1.0}')
	echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
	param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER

	COM_OF_LOSS_T_LONGER=$(awk -v f="$PX4_SIM_SPEED_FACTOR" 'BEGIN{print f * 1.0}')
	echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
	param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER

	COM_DISARM_PRFLT_LONGER=$(awk -v f="$PX4_SIM_SPEED_FACTOR" 'BEGIN{print f * 10.0}')
	echo "COM_DISARM_PRFLT set to $COM_DISARM_PRFLT_LONGER"
	param set COM_DISARM_PRFLT $COM_DISARM_PRFLT_LONGER
fi

# Autostart ID
autostart_file=''
# shellcheck disable=SC2231
for f in ${R}etc/init.d-posix/airframes/"$(param show -q SYS_AUTOSTART)"_*
do
	filename=$(basename "$f")
	case "$filename" in
		*\.*)
			# ignore files that contain a dot (e.g. <vehicle>.post)
			;;
		*)
			autostart_file="$f"
			;;
	esac
done

if [ -e "$autostart_file" ]
then
	. "$autostart_file"

elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ]
then
	echo "Error: no autostart file found ($autostart_file)"
	exit 1
fi

if param greater SYS_AUTOCFG_CAL 0
then
	if [ ${PARAM_RESET_DONE} = yes ]
	then
		param reset CAL_* SYS_AUTOCFG_CAL
	fi
fi

# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value}
env | while IFS='=' read -r line; do
  value=${line#*=}
  name=${line%%=*}
  case $name in
    "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" ;;
  esac
done

dataman start

# only start the simulator if not in replay mode, as both control the lockstep time
if ! replay tryapplyparams
then
	. px4-rc.simulator
fi

load_mon start

if param compare SYS_FAILURE_EN 1
then
	failure_injection_manager start
fi

if param greater SIM_BAT_DRAIN 0
then
	battery_simulator start
fi

system_power_simulator start

tone_alarm start
rc_update start
manual_control start
sensors start
commander start

if ! pwm_out_sim start -m sim
then
	tune_control play error
fi

#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.
. ${R}etc/init.d/rc.vehicle_setup

navigator start

# ROS 2 middleware: PX4_ROS2_MIDDLEWARE (uxrce_dds, zenoh or none) overrides
# ZENOH_ENABLE. uXRCE-DDS runs unless Zenoh is enabled or none is selected.
ros2_middleware=uxrce_dds
case "$PX4_ROS2_MIDDLEWARE" in
	"")
		;;
	uxrce_dds)
		param set ZENOH_ENABLE 0
		;;
	zenoh)
		param set ZENOH_ENABLE 1
		;;
	none)
		param set ZENOH_ENABLE 0
		ros2_middleware=none
		;;
	*)
		echo "WARN [init] Unknown PX4_ROS2_MIDDLEWARE '$PX4_ROS2_MIDDLEWARE', expected uxrce_dds, zenoh or none"
		;;
esac
if param greater -s ZENOH_ENABLE 0
then
	ros2_middleware=zenoh
fi
case "$ros2_middleware" in
	uxrce_dds)
		. px4-rc.uxrce_dds
		;;
	zenoh)
		. px4-rc.zenoh
		;;
esac

if param greater -s MNT_MODE_IN -1
then
	gimbal start
fi

if param greater -s TRIG_MODE 0
then
	camera_trigger start
	camera_feedback start
fi

if param compare -s IMU_GYRO_FFT_EN 1
then
	gyro_fft start
fi

if param compare -s IMU_GYRO_CAL_EN 1
then
	gyro_calibration start
fi

payload_deliverer start

if param compare -s ICE_EN 1
then
	internal_combustion_engine_control start
fi

#user defined mavlink streams for instances can be in PATH
. px4-rc.mavlink

# execute autostart post script if any
[ -e "$autostart_file".post ] && . "$autostart_file".post

#
# State estimator selection.
#
# Started last, right before logging: every estimator update published before the logger opens the
# file is lost, and ekf2 replay needs the log to cover the filter from its very first update. The
# logger cannot move up instead - it drops optional topics that are not advertised yet, and most of
# the modules above own one.
if param compare -s EKF2_EN 1
then
	ekf2 start &
fi

if param compare -s LPE_EN 1
then
	local_position_estimator start
fi

if param compare -s ATT_EN 1
then
	attitude_estimator_q start
fi

# Run script to start logging
if param compare -s EKF2_EN 1
then
	set LOGGER_ARGS "-p ekf2_timestamps"
else
	set LOGGER_ARGS "-p vehicle_attitude"
fi
. ${R}etc/init.d/rc.logging

mavlink boot_complete
replay trystart
