#!/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.
################################################################################


print_configurations () {
  echo "1 Tracking + Stereo (default)"
  echo "2 Tracking Only"
  echo "3 Hires + Stereo + Tracking"
  echo "4 Hires + Tracking"
  echo "5 TOF + Tracking"
  echo "6 Hires + TOF + Tracking"
  echo "7 TOF + Stereo + Tracking"
  echo "8 None"
}

usage () {
  echo "General Usage:"
  echo "voxl-configure-cameras <configuration-id>"
  echo ""
  echo "If no configuration-id is given as an argument, the user will be prompted."
  echo ""
  echo "show this help message:"
  echo "voxl-configure-cameras -h"
  echo ""
  echo "available camera configurations are as follows:"
  print_configurations
  echo ""
  exit 0
}


set -e
CAM_CONFIG_ID=0


if [ "$1" == "-h" ]; then
  usage
  exit 0
elif [ "$1" == "--help" ]; then
  usage
  exit 0
fi


USER=$(whoami)
if [ "${USER}" != "root" ]; then
  echo "Run this script as the root user"
  exit 1
fi


## if no configuration argument given, prompt the user
if [ $# -eq 0 ]; then
  # camera config
  echo ""
  echo "Which camera configuration are you using?"
  print_configurations
  echo ""
  read CAM_CONFIG_ID
else
  CAM_CONFIG_ID=$1
fi

echo "attempting to use camera configuration $CAM_CONFIG_ID"


## configure everything
case $CAM_CONFIG_ID in
1) #Tracking + Stereo
  HIRES_CAM_ID=-1
  TRACKING_CAM_ID=0
  STEREO_CAM_ID=1
  TOF_CAM_ID=-1
  ;;
2) # Tracking Only
  HIRES_CAM_ID=-1
  TRACKING_CAM_ID=0
  STEREO_CAM_ID=-1
  TOF_CAM_ID=-1
  ;;
3) # Hires + Stereo + Tracking
  HIRES_CAM_ID=0
  TRACKING_CAM_ID=1
  STEREO_CAM_ID=2
  TOF_CAM_ID=-1
  ;;
4) # Hires + Tracking
  HIRES_CAM_ID=0
  TRACKING_CAM_ID=1
  STEREO_CAM_ID=-1
  TOF_CAM_ID=-1
  ;;
5) # TOF + Tracking
  HIRES_CAM_ID=-1
  TRACKING_CAM_ID=1
  STEREO_CAM_ID=-1
  TOF_CAM_ID=0
  ;;
6) # Hires + TOF + Tracking
  HIRES_CAM_ID=0
  TRACKING_CAM_ID=2
  STEREO_CAM_ID=-1
  TOF_CAM_ID=1
  ;;
7) # TOF + Stereo + Tracking
  HIRES_CAM_ID=-1
  TRACKING_CAM_ID=0
  STEREO_CAM_ID=1
  TOF_CAM_ID=2
  ;;
8) # none
  HIRES_CAM_ID=-1
  TRACKING_CAM_ID=-1
  STEREO_CAM_ID=-1
  TOF_CAM_ID=-1
  ;;
*)
  echo "invalid option"
  echo "Please provide a camera configuration-id between 1 & 8"
  exit 1;;
esac

sed -E -i "/CAM_CONFIG_ID/c\export CAM_CONFIG_ID=${CAM_CONFIG_ID}" /etc/modalai/camera_env.sh
sed -E -i "/HIRES_CAM_ID/c\export HIRES_CAM_ID=${HIRES_CAM_ID}" /etc/modalai/camera_env.sh
sed -E -i "/TRACKING_CAM_ID/c\export TRACKING_CAM_ID=${TRACKING_CAM_ID}" /etc/modalai/camera_env.sh
sed -E -i "/STEREO_CAM_ID/c\export STEREO_CAM_ID=${STEREO_CAM_ID}" /etc/modalai/camera_env.sh
sed -E -i "/TOF_CAM_ID/c\export TOF_CAM_ID=${TOF_CAM_ID}" /etc/modalai/camera_env.sh

if [ -f /etc/snav/camera.downward.xml ]; then
  echo "adding override_cam_id value=${TRACKING_CAM_ID} to /etc/snav/camera.downward.xml"
  sed -E -i "/override_cam_id/c\    <param name=\"override_cam_id\" value=\"${TRACKING_CAM_ID}\"\/>" /etc/snav/camera.downward.xml
fi

if [ -f /etc/snav/camera.stereo.xml ]; then
  echo "adding override_cam_id value=${STEREO_CAM_ID} to /etc/snav/camera.stereo.xml"
  sed -E -i "/override_cam_id/c\    <param name=\"override_cam_id\" value=\"${STEREO_CAM_ID}\"\/>" /etc/snav/camera.stereo.xml
fi

source /etc/modalai/camera_env.sh

exit 0
