#!/bin/bash
################################################################################
# Copyright 2019 ModalAI Inc.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
#    this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
#    this list of conditions and the following disclaimer in the documentation
#    and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its contributors
#    may be used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# 4. The Software is used solely in conjunction with devices provided by
#    ModalAI Inc.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
################################################################################

CONFIG_FILE=/etc/modalai/voxl-vision-px4.conf
USER=$(whoami)


print_usage () {
	echo "General Usage:"
	echo "voxl-configure-vision-px4"
	echo ""
	echo "To perform factory configuration for VOXL Flight Deck"
	echo "voxl-configure-vision-px4 -f"
	echo ""
	echo "show this help message:"
	echo "voxl-configure-vision-px4 -h"
	echo ""
	exit 0
}


## set most parameters which don't have quotes in json
set_param () {
	if [ "$#" != "2" ]; then
		echo "set_param expected 2 args"
		exit 1
	fi

	# remove quotes if they exist
	var=$1
	var="${var%\"}"
	var="${var#\"}"
	val=$2
	val="${val%\"}"
	val="${val#\"}"

	sed -E -i "/\"$var\":/c\	\"$var\":	$val," ${CONFIG_FILE}
}

## set string parameters which need quotes in json
set_param_string () {
	if [ "$#" != "2" ]; then
		echo "set_param_string expected 2 args"
		exit 1
	fi

	var=$1
	var="${var%\"}"
	var="${var#\"}"

	sed -E -i "/\"$var\":/c\	\"$var\":	\"$2\"," ${CONFIG_FILE}
}

enable_service_and_exit () {
	systemctl daemon-reload
	echo "enabling voxl-vision-px4 systemd service"
	systemctl enable voxl-vision-px4.service
	echo "starting voxl-vision-px4 systemd service"
	systemctl start voxl-vision-px4.service
	echo "DONE configuring voxl-vision-px4"
	exit 0
}

## print help message if requested
if [ "$1" == "-h" ]; then
	print_usage
	exit 0
elif [ "$1" == "--help" ]; then
	print_usage
	exit 0
fi

## sanity checks
if [ "${USER}" != "root" ]; then
	echo "Run this script as the root user"
	exit 1
fi

# make sure modalai-vl is new enough
if [ ! -f /usr/bin/voxl-configure-modalai-vl ]; then
	echo "ERROR, can't find /usr/bin/voxl-configure-modalai-vl"
	echo "make sure modalai-vl is installed and up to date"
	exit 1
fi

# have the voxl-vision-px4 binary open and close the config file
# this will clean it up and add any new values from updates
echo "Current parameters before config:"
voxl-vision-px4 -c
echo "^ don't worry about the segfault ^"
echo " "

## perform factory setup if requested
if [ "$1" == "-f" ]; then
	# configure vl for flight desk
	bash /usr/bin/voxl-configure-modalai-vl -f
	# copy over default config file
	cp /share/modalai/voxl-vision-px4.conf ${CONFIG_FILE}
	enable_service_and_exit
}


## configure the vision lib, this also calls voxl-configure-cameras
bash /usr/bin/voxl-configure-modalai-vl

## copy over default config file if missing or old
if [ ! -f ${CONFIG_FILE} ]; then
	echo "Making new config file ${CONFIG_FILE}"
	cp /share/modalai/voxl-vision-px4.conf ${CONFIG_FILE}
elif ! grep -q "qgc_ip" ${CONFIG_FILE}; then
	echo ""
	echo "detected an old version of ${CONFIG_FILE} that will be"
	echo "replaced. We suggest saving any changes you made to this file such as"
	echo "the IP address and reapplying them after this update."
	echo "Press enter to continue. THIS WILL OVERWRITE ${CONFIG_FILE}"
	read
	cp -f /share/modalai/voxl-vision-px4.conf ${CONFIG_FILE}
fi

## ask qgc_ip question
echo " "
echo "Now we are going to do a preliminary configuration of ${CONFIG_FILE}"
echo " "
echo "Which IP address is your QGroundStation at?"
echo "just press enter to leave this option as-is"
read QGCIP
if [ "${QGCIP}" != "" ]; then
	echo "setting qgc_ip to ${QGCIP}"
	set_param qgc_ip ${QGCIP}
fi

## ask vio_question
echo " "
echo "Do you want to enable Visual Inertial Odometry (VIO)?"
select opt in "yes" "no"; do
case $opt in
yes )
	set_param en_vio true
	echo "VIO enabled, make sure EKF2_AID_MASK and EKF2_HGT_MODE are set"
	echo "to enable vision in PX4. Press enter to continue."
	read
	break;;
no )
	set_param en_vio false
	echo "VIO disabled. Make sure EKF2_AID_MASK and EKF2_HGT_MODE are set"
	echo "in PX4 to disable vision estimates. Press enter to continue."
	read
	break;;
*)
	echo "invalid option"
	esac
done


## ask VOA question
echo " "
echo "Do you want to enable stereo cameras and visual obstacle avoidance?"
select opt in "yes" "no"; do
case $opt in
yes )
	set_param en_voa true
	## warn user that stereo calibration file is missing
	if [ ! -f /etc/snav/calibration.stereo.xml ]; then
		echo "WARNING: Missing stereo calibration file. Follow instructions here to"
		echo "calibrate if you wish to use stereo visual obstacle avoidance."
		echo "https://docs.modalai.com/calibrate-cameras/"
		echo ""
		echo "press enter to continue anyway, VOA WONT WORK UNTIL CALIBRATED"
		read
	fi
	echo "You will need need to configure the CP_DIST parameter in PX4 after voxl-vision-px4"
	echo "is up and running. Press enter to continue."
	read
	break;;
no )
	set_param en_voa false
	break;;
*)
	echo "invalid option"
	esac
done

## ask figure eight question
echo " "
echo "Do you want to enable voxl-vision-px4 to command PX4"
echo "to fly a figure eight path when flipped into offboard mode?"
echo "Don't enable this if you intend to do your own offboard"
echo "control via MAVROS or similar."
select opt in "yes" "no"; do
case $opt in
yes )
	set_param_string offboard_mode "figure_eight"
	break;;
no )
	set_param_string offboard_mode "off"
	break;;
*)
	echo "invalid option"
	esac
done

## ask IMU location question
echo " "
echo "What is the location of VOXL's VIO IMU with respect to the drone's"
echo "center of mass? X-forward, Y-right, Z-down. Make sure PX4's vision"
echo "offset parameters EKF2_EV_POS_X,Y,Z parameters are set to 0!!"
echo "voxl-vision-px4 will handle this offset from now on."
select opt in "M500" "Seeker" "Cindy" "Custom1" "Custom"; do
case $opt in
M500 )
	set_param T_imu_wrt_body "[0.068, -0.015, -0.008]"
	break;;
Seeker )
	set_param T_imu_wrt_body "[0.0357, -0.015, -0.030]"
	break;;
Cindy )
	set_param T_imu_wrt_body "[0.150, -0.015, -0.040]"
	break;;
Custom1 )
	set_param T_imu_wrt_body "[0.108, -0.002, -0.019]"
	break;;
Custom )
	echo "Please enter X value, distance (meters) forward of center of mass"
	read X
	echo "Please enter Y value, distance (meters) right of center of mass"
	read Y
	echo "Please enter Z value, distance (meters) above center of mass"
	read Z
	set_param T_imu_wrt_body "[${X}, ${Y}, ${Z}]"
	break;;
*)
	echo "invalid option"
	esac
done

## ask IMU rotation question
echo " "
echo "What is the rotation of VOXL's VIO IMU to the drone's body frame?"
echo "Body frame is X-forward, Y-right, Z-down."
echo "see: https://docs.modalai.com/camera-imu-coordinate-frames/"
echo "default is: R_imu_to_body:  [[0, 1, 0], [1, 0, 0], [0, 0, -1]]"
echo "Most drones will use the default rotation as used by M500 reference."
select opt in "Default" "Custom1" ; do
case $opt in
Default )
	set_param R_imu_to_body "[[0, 1, 0], [1, 0, 0], [0, 0, -1]]"
	break;;
Custom1 )
	set_param R_imu_to_body "[[0, 1, 0], [0, 0, 1], [1, 0, 0]]"
	break;;
*)
	echo "invalid option"
	esac
done

## ask body height question
echo " "
echo "What is the height of the drone's center of mass above floor in meters?"
echo
select opt in "M500_short_4cm" "M500_tall_20cm" "Seeker" "Cindy" "Custom1" "Custom"; do
case $opt in
M500_short_4cm )
	set_param height_body_above_ground_m 0.040
	break;;
M500_tall_20cm)
	set_param height_body_above_ground_m 0.200
	break;;
Seeker)
	set_param height_body_above_ground_m 0.035
	break;;
Cindy )
	set_param height_body_above_ground_m 0.120
	break;;
Custom1 )
	set_param height_body_above_ground_m 0.030
	break;;
Custom )
	echo "Please enter positive height in meters"
	read Z
	set_param height_body_above_ground_m ${Z}
	break;;
*)
	echo "invalid option"
	esac
done

## ask stereo questions
echo " "
echo "how far in front of the center of mass is the stereo camera pair?"
echo
select opt in "M500" "Seeker" "Cindy" "Custom1" "Custom"; do
case $opt in
M500 )
	set_param T_stereo_wrt_body "[0.10, -0.04, 0.0]"
	break;;
Seeker )
	set_param T_stereo_wrt_body "[0.08, -0.04, -0.02]"
	break;;
Cindy)
	set_param T_stereo_wrt_body "[0.17, -0.04, -0.03]"
	break;;
Custom1 )
	set_param T_stereo_wrt_body "[0.16, -0.04, 0.0]"
	break;;
Custom )
	echo "Please enter positive distance in meters"
	read X
	set_param T_stereo_wrt_body "[${X}, -0.04, 0.0]"
	break;;
*)
	echo "invalid option"
	esac
done

# all done!
enable_service_and_exit

