#! /bin/bash

echo " "
echo "Script for running ORB_SLAM3 on Monocular mono-kitti dataset."
echo "--------------------------------------------------------"

# ORB_SLAM3 Directories
pathDatasetMono_KITTI='/home/ivan/ivan/git/work_drivecast2/SLAM/datasets/mono-kitti/sequences' #Example, it is necesary to change it in your case
orb_slam3_dir="/home/ivan/ivan/git/work_drivecast2/SLAM/ORB_SLAM3-1.0-release"

echo "KITTI dataset path is: $pathDatasetMono_KITTI"
echo "ORB_SLAM3 directory is: $orb_slam3_dir"
echo "--------------------------------------------------------"

function show_help {
  echo "--------------------------------------------------------"
	echo "Example of usage: -d=00 or --dataset=00. They are used as the
	  folder names in tum-vi dataset folder. Output file doesn't change the name. Don't forget to rename and move the
	  result before the next run."
}
 
if [[ $1 == -h || $1 == --help ]] ; then
	show_help
 	exit 0
fi

for i in "$@"; do
	case "$i" in
	  -d=*|--dataset=*)
	    export DATASET="${i#*=}"
	    export DATASET_DIR=$pathDatasetMono_KITTI/$DATASET
	    shift
	    ;;
	  -*|--*)
	    echo "Unknown option $i"
	    show_help
	    exit 1
	    ;;
	  *)
	    echo "Invalid usage."
	    show_help
	    exit 1
	    ;;
	esac
done

#------------------------------------
# Monocular Examples

#Extract the name of the folder with the timestamps in tum-vi dataset. 
COPY_DATASET=${DATASET}
LEN=${#COPY_DATASET}
for ((i=$LEN-1;i>=0;i--)) ; do REV="$REV${COPY_DATASET:$i:1}"; done

REV_CUTTED=${REV:3}
LEN=${#REV_CUTTED}
for ((i=$LEN-1;i>=0;i--)) ; do TIMESTAMPS_FILENAME="$TIMESTAMPS_FILENAME${REV_CUTTED:$i:1}"; done
echo "The filename is the following: $TIMESTAMPS_FILENAME"

REV_ATLAS_CUTTED=${REV:7}
LEN=${#REV_ATLAS_CUTTED}
for ((i=$LEN-1;i>=0;i--)) ; do ATLAS_NAME="$ATLAS_NAME${REV_ATLAS_CUTTED:$i:1}"; done
ATLAS_NAME="${ATLAS_ANME:8}Atlas.osa"
echo "The atlas name is the following: $ATLAS_NAME"

echo "Launching $DATASET with Monocular sensor"
echo "Dataset directory is: $DATASET_DIR"
"$orb_slam3_dir"/Examples/Monocular/mono_kitti "$orb_slam3_dir"/Vocabulary/ORBvoc.txt "$orb_slam3_dir"/Examples/Monocular/KITTI04-12.yaml "$DATASET_DIR"

mv "f_${DATASET}_mono.txt" "$DATASET"/"f_${DATASET}_mono_orb_slam3.txt"
mv "kf_${DATASET}_mono.txt" "$DATASET"/"kf_${DATASET}_mono_orb_slam3.txt"
mv "f_ ${DATASET}_mono_raw.txt" "$DATASET"/"f_${DATASET}_mono_orb_slam3_raw.txt"
mv "$ATLAS_NAME" "$DATASET/$ATLAS_NAME"