#!/bin/bash
################################################################################
# Copyright 2025 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.
################################################################################

NAME="voxl-vision-hub"
SERVICE_FILE="${NAME}.service"
CONFIG_FILE="/etc/modalai/${NAME}.conf"
OLD_CONFIG_FILE="/etc/modalai/voxl-vision-px4.conf"
USER=$(whoami)

#
# TBD: to be moved to new flight manager
#
CONFIG_VFC_FILE="/etc/modalai/vfc.conf"


print_usage () {
  echo ""
  echo "Start wizard with prompts:"
  echo "voxl-configure-vision-hub"
  echo ""
  echo "Shortcut configuration arguments for scripted setup."
  echo "factory_enable will reset the config file to factory defaults"
  echo "before enabling the service."
  echo ""
  echo "voxl-configure-vision-hub wizard"
  echo "voxl-configure-vision-hub enable"
  echo "voxl-configure-vision-hub disable"
  echo "voxl-configure-vision-hub factory_disable"
  echo "voxl-configure-vision-hub factory_enable"
  echo "voxl-configure-vision-hub factory_enable_figure_eight"
  echo "voxl-configure-vision-hub factory_enable_no_figure_eight"
  echo "voxl-configure-vision-hub factory_enable_trajectory"
  echo "voxl-configure-vision-hub factory_enable_vfc_fpv"
  echo "voxl-configure-vision-hub factory_enable_vfc_fpv_flow"
  echo "voxl-configure-vision-hub factory_enable_vfc_starling"
  echo "voxl-configure-vision-hub factory_enable_vfc_starling_flow"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow_steam_deck"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow_d0008"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow_d0008v4"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow_d0013"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow_d0014"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow_d0019"
  echo "voxl-configure-vision-hub factory_enable_vfc_pos_back_flow_d0020"
  echo "voxl-configure-vision-hub enable_mpc_vio"
  echo "voxl-configure-vision-hub disable_mpc_vio"
  echo "voxl-configure-vision-hub update_vfc_settings"
  echo ""
  echo "show this help message:"
  echo "voxl-configure-vision-hub help"
  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}
}


#
# TBD: to be moved to new flight manager
#
set_vfc_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_VFC_FILE}
}

#
# TBD: to be moved to new flight manager
#
## set string parameters which need quotes in json
set_vfc_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_VFC_FILE}
}


disable_service_and_exit () {
  echo "disabling ${NAME} systemd service"
  systemctl disable ${SERVICE_FILE}
  echo "stopping ${NAME} systemd service"
  systemctl stop ${SERVICE_FILE}
  echo "Done configuring ${NAME}"
  exit 0
}

enable_service_and_exit () {
  echo "enabling  ${NAME} systemd service"
  systemctl enable  ${SERVICE_FILE}
  echo "Done configuring ${NAME}"
  echo "service will start on next reboot"
  exit 0
}

reset_config_file_to_default () {
  echo "wiping old config file"
  rm -rf ${CONFIG_FILE}
  rm -rf ${CONFIG_VFC_FILE}
  ${NAME} -c
}


move_old_file_if_necessary () {
  if [ -f $OLD_CONFIG_FILE ];then
    if [ ! -f $CONFIG_FILE ]; then
      echo "NOTE: moving old config file to new location: $CONFIG_FILE"
      mv $OLD_CONFIG_FILE $CONFIG_FILE
    else
      echo "NOTE: deleting old config file"
      rm $OLD_CONFIG_FILE
    fi
  fi
}




################################################################################
## actual start of execution, handle optional arguments first
################################################################################

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


## convert argument to lower case for robustness
arg=$(echo "$1" | tr '[:upper:]' '[:lower:]')

## parse arguments
case ${arg} in
  "")
    echo "ERROR must provide an argument"
    print_usage
    exit 1
    ;;
  "h"|"-h"|"help"|"--help")
    print_usage
    exit 0
    ;;
  "wizard"|"-w"|"--wizard")
    ;;
  "enable")
    enable_service_and_exit
    ;;
  "disable")
    disable_service_and_exit
    ;;
  "factory_disable")
    reset_config_file_to_default
    disable_service_and_exit
    ;;
  "factory_enable")
    reset_config_file_to_default
    enable_service_and_exit
    ;;
  "factory_enable_figure_eight")
    reset_config_file_to_default
    set_param_string offboard_mode "figure_eight"
    enable_service_and_exit
    ;;
  "factory_enable_no_figure_eight")
    reset_config_file_to_default
    set_param_string offboard_mode "off"
    enable_service_and_exit
    ;;
  "factory_enable_trajectory")
    reset_config_file_to_default
    set_param_string offboard_mode "trajectory"
    enable_service_and_exit
    ;;
  "factory_enable_backtrack")
    reset_config_file_to_default
    set_param_string offboard_mode "backtrack"
    enable_service_and_exit
    ;;
  "factory_enable_vfc_fpv")
    reset_config_file_to_default
    set_param_string offboard_mode "vfc"
    set_param en_vio "false"
    set_vfc_param vfc_thrust_hover 0.23
        set_vfc_param vfc_points_min 0
        set_vfc_param vfc_disable_fallback "true"
        enable_service_and_exit
    ;;
  
  #######################
  # FOLLOWING ARE ONLY VFC APPLICATIONS  
  #######################
  "factory_enable_vfc_starling")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "false"
        
        set_vfc_param vfc_thrust_hover 0.37
        set_vfc_param vfc_disable_fallback "false"
        set_vfc_param backtrack_rc_chan 8
        
        enable_service_and_exit
    ;;
    
  "factory_enable_vfc_fpv_flow")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
                    set_param en_vio "false"
        set_param vfc_thrust_hover 0.23
        set_param vfc_points_min 0
        set_param vfc_disable_fallback "false"
        set_param vfc_position_mode_rc_max 0
        set_param vfc_flow_mode_rc_max 2100
        enable_service_and_exit
    ;;

  "factory_enable_vfc_starling_flow")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
                    set_param en_vio "false"
            
        set_param vfc_thrust_hover 0.37
        set_param vfc_points_min 0
        set_param vfc_disable_fallback "false"
        set_param vfc_position_mode_rc_max 0
        set_param vfc_flow_mode_rc_max 2100
        enable_service_and_exit
    ;;
    
  "factory_enable_vfc_pos_back_flow")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
                    set_param en_vio "false"
            
        set_param vfc_thrust_hover 0.23
        set_param vfc_points_min 0
        set_param vfc_disable_fallback "false"
        set_param vfc_position_mode_rc_min 1300
        set_param vfc_position_mode_rc_max 1700
        set_param vfc_flow_mode_rc_min 0
        set_param vfc_flow_mode_rc_max 0
        set_param backtrack_rc_chan 10
        
        set_vfc_param vfc_kp_xy 0.64
        set_vfc_param vfc_kd_xy 2.45
        set_vfc_param vfc_kp_xy_vio 3.5
        set_vfc_param vfc_kd_xy_vio 3.5
        
        set_vfc_param vfc_kp_z 0.4
        set_vfc_param vfc_kd_z 0.12
        set_vfc_param vfc_kp_z_vio 0.4 
        set_vfc_param vfc_kd_z_vio 0.12
        
        set_vfc_param vfc_xy_acc_limit_vio 2.61
        
        enable_service_and_exit
    ;;
      
    "d0008"|"factory_enable_vfc_pos_back_flow_d0008")

        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "false"
        set_vfc_param vfc_thrust_hover 0.23
        enable_service_and_exit
    ;;

    "d0008v4"|"factory_enable_vfc_pos_back_flow_d0008v4")

        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "false"
        set_vfc_param vfc_thrust_hover 0.23
        set_vfc_param vfc_q_min 5 # PENDING FUTURE TESTING TO CONFIRM
        enable_service_and_exit
    ;;
    "d0013"|"factory_enable_vfc_pos_back_flow_d0013")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "false"
        set_vfc_param vfc_thrust_hover 0.238694
        enable_service_and_exit
    ;;
    
    "d0014"|"factory_enable_vfc_pos_back_flow_d0014")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "true"
        set_param tracking_trigger_ch 7 
        set_vfc_param vfc_thrust_hover 0.39
        enable_service_and_exit
    ;;
    
    "d0019"|"factory_enable_vfc_pos_back_flow_d0019")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "false"
        set_vfc_param vfc_thrust_hover 0.125
        enable_service_and_exit
    ;;
    
    "d0020"|"factory_enable_vfc_pos_back_flow_d0020")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "false"
        set_vfc_param vfc_thrust_hover 0.18
        enable_service_and_exit
    ;;

  "factory_enable_vfc_pos_back_flow_steam_deck")
        reset_config_file_to_default
        set_param_string offboard_mode "vfc"
        set_param en_vio "false"
        set_vfc_param vfc_thrust_hover 0.23
        set_vfc_param vfc_position_mode_rc_min 1700
        set_vfc_param vfc_position_mode_rc_max 2100
        set_vfc_param vfc_flow_mode_rc_min 0
        set_vfc_param vfc_flow_mode_rc_max 0
        set_vfc_param backtrack_rc_chan 5
        set_vfc_param vfc_submode_ch 7
        enable_service_and_exit
    ;;
    
  "enable_mpc_vio")
    set_param en_vio "true"
    ;;
  "disable_mpc_vio")
    set_param en_vio "false"
    ;;
  "configure_hitl")
    set_param en_hitl "true"
    set_vfc_param vfc_thrust_hover 0.6
    set_vfc_param vfc_kp_z_vio 0.7
    set_vfc_param vfc_ki_z_vio 0.6
    set_vfc_param vfc_kd_z_vio 0.35
	exit 0
    ;;
  "configure_hitl_joystick")
    set_vfc_param vfc_submode_ch 7
    set_vfc_param backtrack_rc_chan 6
	exit 0
    ;;
  "update_vfc_settings")
        HOVER=`cat /data/HOVER_THRUST.dat`
        echo " "
        echo "Setting VFC thrust_hover to $HOVER"
    set_vfc_param vfc_thrust_hover $HOVER
    enable_service_and_exit
    ;;
  *)
    echo "invalid option"
    print_usage
    exit 1
esac



################################################################################
## no optional arguments, start config wizard prompts
################################################################################

clear -x
echo "Starting voxl-vision-hub wizard"
echo " "
echo "Do you want to reset the config file to factory defaults?"
select opt in "yes" "no"; do
case $opt in
yes )
  reset_config_file_to_default
  break;;
no )
  echo "loading and updating config file with ${NAME} -c"
  ${NAME} -c
  break;;
*)
  echo "invalid option"
  esac
done


echo " "
echo "Which offboard demo mode would you like to enable?"
echo "select off if you intend to do your own offboard"
echo "control via MAVROS or similar."
select opt in "figure_eight" "trajectory" "off"; do
case $opt in
figure_eight )
  set_param_string offboard_mode "figure_eight"
  break;;
trajectory )
  set_param_string offboard_mode "trajectory"
  break;;
off )
  set_param_string offboard_mode "off"
  break;;
*)
  echo "invalid option"
  esac
done




# all done!
enable_service_and_exit

