#!/bin/sh

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

SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"

#
# Main SITL startup script
#

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

# initialize script variables
set AUX_MODE                    none
set IO_PRESENT                  no
set LOG_FILE                    bootlog.txt
set MAV_TYPE                    none
set MIXER                       none
set MIXER_AUX                   none
set MIXER_FILE                  none
set OUTPUT_MODE                 sim
set PWM_OUT                     none
set SDCARD_MIXERS_PATH          etc/mixers
set USE_IO                      no
set VEHICLE_TYPE                none

set RUN_MINIMAL_SHELL           no

# Use the variable set by sitl_run.sh to choose the model settings.
if [ "$PX4_SIM_MODEL" = "shell" ]; then
	set RUN_MINIMAL_SHELL yes
else
	# Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL})
	# shellcheck disable=SC2012 #(the file names don't contain spaces)
	REQUESTED_AUTOSTART=$(ls "$SCRIPT_DIR" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p')
	if [ -z "$REQUESTED_AUTOSTART" ]; then
		echo "Error: Unknown model '$PX4_SIM_MODEL'"
		exit -1
	fi
fi

# clear bootlog
[ -f $LOG_FILE ] && rm $LOG_FILE


uorb start
if [ -f eeprom/parameters ]
then
	param load
fi

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


# Use environment variable PX4_ESTIMATOR to choose estimator.
if [ "$PX4_ESTIMATOR" = "ekf2" ]; then
	param set SYS_MC_EST_GROUP 2
elif [ "$PX4_ESTIMATOR" = "lpe" ]; then
	param set SYS_MC_EST_GROUP 1
elif [ "$PX4_ESTIMATOR" = "inav" ]; then
	param set SYS_MC_EST_GROUP 0
fi

if param compare SYS_AUTOSTART $REQUESTED_AUTOSTART
then
	set AUTOCNF no
else
	set AUTOCNF yes
fi

# multi-instance setup
# shellcheck disable=SC2154
param set MAV_SYS_ID $((1+px4_instance))
simulator_udp_port=$((14560+px4_instance))
udp_offboard_port_local=$((14557+px4_instance))
udp_offboard_port_remote=$((14540+px4_instance))
udp_gcs_port_local=$((14556+px4_instance))

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

	param set BAT_N_CELLS 3
	param set CAL_ACC0_ID 1376264
	param set CAL_ACC0_XOFF 0.01
	param set CAL_ACC0_XSCALE 1.01
	param set CAL_ACC0_YOFF -0.01
	param set CAL_ACC0_YSCALE 1.01
	param set CAL_ACC0_ZOFF 0.01
	param set CAL_ACC0_ZSCALE 1.01
	param set CAL_ACC1_ID 1310728
	param set CAL_ACC1_XOFF 0.01
	param set CAL_GYRO0_ID 2293768
	param set CAL_GYRO0_XOFF 0.01
	param set CAL_MAG0_ID 196616
	param set CAL_MAG0_XOFF 0.01
	param set SENS_DPRES_OFF 0.001
	param set CBRK_AIRSPD_CHK 0

	param set COM_DISARM_LAND 3
	param set COM_OBL_ACT 2
	param set COM_OBL_RC_ACT 0
	param set COM_OF_LOSS_T 5
	param set COM_RC_IN_MODE 1

	param set EKF2_AID_MASK 1
	param set EKF2_ANGERR_INIT 0.01
	param set EKF2_GBIAS_INIT 0.01
	param set EKF2_HGT_MODE 0
	param set EKF2_MAG_TYPE 1

	param set MC_PITCH_P 6
	param set MC_PITCHRATE_P 0.2
	param set MC_ROLL_P 6
	param set MC_ROLLRATE_P 0.2
	param set MIS_TAKEOFF_ALT 2.5
	param set MPC_ALT_MODE 0
	param set MPC_HOLD_MAX_Z 2.0
	param set MPC_Z_VEL_I 0.15
	param set MPC_Z_VEL_P 0.6
	param set NAV_ACC_RAD 2.0
	param set NAV_DLL_ACT 2
	param set RTL_DESCEND_ALT 5.0
	param set RTL_LAND_DELAY 5
	param set RTL_RETURN_ALT 30.0

	param set SDLOG_DIRS_MAX 7
	param set SENS_BOARD_ROT 0
	param set SENS_BOARD_X_OFF 0.000001
	param set SYS_RESTART_TYPE 2

	# LPE: GPS only mode
	param set LPE_FUSION 145

	param set WEST_EN 0
fi

# Autostart ID
autostart_file_match="etc/init.d-posix/$(param show -q SYS_AUTOSTART)_*"
# shellcheck disable=SC2206
autostart_files=( $autostart_file_match )
autostart_file="${autostart_files[0]}" # use first match, but there should really only be one
if [ ! -e "$autostart_file" ]; then
	echo "Error: no autostart file found"
	exit -1
fi

sh "$autostart_file"


dataman start
replay tryapplyparams
simulator start -s -u $simulator_udp_port
tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start
sensors start
commander start
navigator start

if param compare WEST_EN 1
then
	wind_estimator start
fi

if ! param compare MNT_MODE_IN -1
then
	vmount start
fi

if param greater TRIG_MODE 0
then
	camera_trigger start
	param set CAM_FBACK_MODE 1
	camera_feedback start
fi


if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ]
then
	if param compare CBRK_AIRSPD_CHK 0
	then
		measairspeedsim start
	fi
fi

# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
#       rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps.
#
sh etc/init.d/rc.vehicle_setup

# GCS link
mavlink start -x -u $udp_gcs_port_local -r 4000000
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local
mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local
mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local
mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local
mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local

# API/Offboard link
mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote

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


logger start -e -t -b 1000

mavlink boot_complete
replay trystart