pushd `dirname $0` > /dev/null
MY_DIR=`pwd`
popd > /dev/null
source $MY_DIR/_inc_constants.sh

function prepareSettingsFile() {
	echo "================ PREPARING SETTINGS FILE ================";
	echo "Experiment name: ${EXPERIMENT_NAME}";
	echo "Num of worksites: ${NUM_WORKSITES}";
	echo "Num of robots: ${NUM_ROBOTS}"
	
	#-- mission type
	missionNo=0
	if [ "${MISSION_TYPE}" == "TA" ] ; then
		missionNo=0
	elif [ "${MISSION_TYPE}" == "FOR" ] ; then
		missionNo=1
	else
		echo "!!! Incorrect mission type, specify FOR or TA, got '${MISSION_TYPE}'"
		exit 0
	fi
	echo "Mission type: ${MISSION_TYPE} (no ${missionNo})"
	
	#-- controller type
	controllerNo=0
	if [ "${CONTROLLER_TYPE}" == "S" ] ; then
		controllerNo=0
	elif [ "${CONTROLLER_TYPE}" == "LB" ] ; then
		controllerNo=1
	elif [ "${CONTROLLER_TYPE}" == "BEE" ] ; then
		controllerNo=2
	elif [ "${CONTROLLER_TYPE}" == "RT" ] ; then
		controllerNo=3
	else
		echo "!!! Incorrect controller type, specify S, LB or BEE, RT got '${CONTROLLER_TYPE}'"
		exit 0
	fi
	echo "Controller type: ${CONTROLLER_TYPE} (no ${controllerNo})"
	
	#-- environment
	minWorksiteD=${WORKSITE_DISTANCE}
	maxWorksiteD=$((minWorksiteD+5000))
	worksiteVolume=$((48/NUM_WORKSITES))
#worksiteVolume=5
	environmentName="heap"
	if [ "${NUM_WORKSITES}" -gt "4" ]; then
		environmentName="scatter"
		maxWorksiteD=$((minWorksiteD+5000))
	fi
	environmentName="${environmentName}${NUM_WORKSITES}-${WORKSITE_DISTANCE}"
	echo "Environment name: ${environmentName}";
	echo "Worksite D: ${minWorksiteD} - ${maxWorksiteD}"
	echo "Worksite volume: ${worksiteVolume}"
	
	#-- make sure directories exist
	cd ${RESULTS_DIRECTORY}
	RUN_DIRECTORY=${EXPERIMENT_NAME}
	if [ -d ${RUN_DIRECTORY} ]; then
		echo "   directory ${RUN_DIRECTORY} found"
	else
		mkdir ${RUN_DIRECTORY}
		echo "   created directory ${RUN_DIRECTORY}"
	fi
	
	cd ${RUN_DIRECTORY}
		
	tempDirName="NR${NUM_ROBOTS}_${MISSION_TYPE}_${environmentName}_${CONTROLLER_TYPE}"
	if [ "${CONTROLLER_TYPE}" == "LB" ] ; then
		tempDirName="${tempDirName}_commD${COMM_D}"
	fi
	
	if [ -d ${tempDirName} ]; then
		echo "   directory ${tempDirName} found"
	else
		mkdir ${tempDirName}
		echo "   created directory ${tempDirName}"
	fi
	RUN_DIRECTORY=${RUN_DIRECTORY}/${tempDirName}
	
	
	#-- find run no
	cd ${tempDirName}
	runNo=0
	runNoFound=0
	while [ $runNoFound -eq 0 ]; do
		fileName="run${runNo}_infoEvents.txt"
		if [ -f ${fileName} ]; then
			#-- file already exists - trying a higher run number
			((runNo++))
		else
			#-- file doesnt exist, correct runno
			runNoFound=1
			echo "   new run no: ${runNo}"
		fi
	done
	
	RUN_SETTINGS_FILE="${RESULTS_DIRECTORY}/${RUN_DIRECTORY}/run${runNo}_settings.txt"
	RUN_RESULT_FILES_COMMON="${RESULTS_DIRECTORY}/${RUN_DIRECTORY}/run${runNo}_"
	echo "Settings file: ${RUN_SETTINGS_FILE}"
	
	LOG_FILE_PREFIX=${EXPERIMENT_NAME}_${tempDirName}_run${runNo}
	echo "Log file prefix: ${LOG_FILE_PREFIX}"
			
	#-- go back to script directory
	cd ../../../../scripts
	
	#-- create the settings file and replace the parameter placeholders with values
	sed -e "s^\[NUM_WORKSITES\]^${NUM_WORKSITES}^g;\
	    s^\[CONTROLLER_NO\]^${controllerNo}^g;\
	    s^\[MIN_WORKSITE_D\]^${minWorksiteD}^g;\
		s^\[MAX_WORKSITE_D\]^${maxWorksiteD}^g;\
	    s^\[MISSION_NO\]^${missionNo}^g;\
	    s^\[WORKSITE_VOLUME\]^${worksiteVolume}^g;\
		s^\[LOG_FILE_PREFIX\]^${LOG_FILE_PREFIX}^g;\
		s^\[COMM_D\]^${COMM_D}^g;\
		" \
		${RUN_SETTINGS_FILE_TEMPLATE} > ${RUN_SETTINGS_FILE}
	
	#-- add all possible robot ids to the file
	echo "" >> ${RUN_SETTINGS_FILE}
	for robotId in ${POSSIBLE_ROBOT_IDS[@]}; do
		echo "robotId ${robotId}" >> ${RUN_SETTINGS_FILE}
	done	
	echo "=========================================================";
}


function deployToRobots() {
	#-- move extra files
	for robotId in ${ROBOT_IDS[@]}; do
		sed -e "s^\[THIS_ROBOT_ID\]^${robotId}^g;\
				" \
				${RUN_SETTINGS_FILE} > ../_temp/runSettings_robot${robotId}.txt
	done
	
	#-- copy settings to robots
	for robotId in ${ROBOT_IDS[@]}; do
		echo ">>>>>> Deploying new settings to robot ${robotId}"
		ssh root@192.168.20.${robotId} "rm ${ONROBOT_WORKING_DIR_PATH}${ONROBOT_FOLDER_NAME}/bin/runSettings.txt"
		rsync -lv --progress ../_temp/runSettings_robot${robotId}.txt root@192.168.20.${robotId}:${ONROBOT_WORKING_DIR_PATH}${ONROBOT_FOLDER_NAME}/bin/runSettings.txt < /dev/null &
	done
	
	#-- check if settings are there
	keepChecking=1
	allRobotsHaveCode=0
	while [ $keepChecking -eq 1 ]
	do
		allRobotsHaveCode=1
		for robotId in ${ROBOT_IDS[@]}; do
			if ssh root@192.168.20.${robotId} stat ${ONROBOT_WORKING_DIR_PATH}${ONROBOT_FOLDER_NAME}/bin/runSettings.txt \> /dev/null 2\>\&1
			then
				echo "   ... Robot ${robotId} done"
			else
				echo "   ... Robot ${robotId} waiting"
				allRobotsHaveCode=0
			fi
		done
		
		if [ $allRobotsHaveCode -eq 1 ]; then
			keepChecking=0
		fi
	done
}

function runRobots() {
	for robotId in ${ROBOT_IDS[@]}; do
		echo ">>>>>> RUNNING ON ROBOT ${robotId}"
		ssh root@192.168.20.${robotId} "cd ${ONROBOT_WORKING_DIR_PATH}/${ONROBOT_FOLDER_NAME}/bin; ./Controller" &
	done
		
}

function runObserver() {
	# kill all potentially running processes
	pkill Epuck
	
	# copy settings file
	cp ${RUN_SETTINGS_FILE} ${OBSERVER_BIN}/runSettings.txt
	
	#run
	cd ${OBSERVER_BIN}
	roslaunch cpp_observer test.launch &
	cd ../../scripts
}

#-- constants. all paths are relative to this file, which is placed in scripts
ONROBOT_WORKING_DIR_PATH="code_lenka/"
ONROBOT_FOLDER_NAME="taskDiscovery"
ROBOT_BIN=../cpp_robot
OBSERVER_BIN=../cpp_observer/launch
RESULTS_DIRECTORY=../experiments/results

#-- variables
EXPERIMENT_NAME=$1 #specified which template will be used, e.g. for "test" template experiments/runSettings_test.txt will be used. Also specifies main folder where results will be put
MISSION_TYPE=$2 # FOR, TA
NUM_WORKSITES=$3
WORKSITE_DISTANCE=$4
CONTROLLER_TYPE=$5 #S, LB, BEE
COMM_D=$6 # communication distance
POSSIBLE_ROBOT_IDS=(1 2 3 4 5 8 11 30 36 37 38 39) #alrobot ids that have been setup to work - a robot might be exchanged for another during experiment if one breaks

NUM_ROBOTS=${#ROBOT_IDS[@]}
RUN_SETTINGS_FILE_TEMPLATE=../experiments/runSettings_${EXPERIMENT_NAME}.txt
RUN_DIRECTORY=""
RUN_SETTINGS_FILE=""
RUN_RESULT_FILES_COMMON=""
LOG_FILE_PREFIX=""
#--

		
#-- check params
if [ "${EXPERIMENT_NAME}" == "" ] || [ "${MISSION_TYPE}" == "" ] || [ "${NUM_WORKSITES}" == "" ] || [ "${WORKSITE_DISTANCE}" == "" ] || [ "${CONTROLLER_TYPE}" == "" ]; then
	echo "!!!! Specify arguments: experiment name, mission type (FOR / TA) num of worksites, worksite distance, controller type (S / LB / BEE), [optional] communicatino distance as e.g. ./doExperiment.sh test FOR 3 5000 S 6000"
	exit 0
fi
if [ ! -f ${RUN_SETTINGS_FILE_TEMPLATE} ]; then
	echo "!!!! Experiment template '${RUN_SETTINGS_FILE_TEMPLATE} does not exist"
	exit 0
fi


if [ "${COMM_D}" == "" ]; then
	COMM_D=12500
fi
#--

prepareSettingsFile
deployToRobots
runObserver
runRobots

echo ">>>>>> Experiment running"
#-- trap ctrl-c and call ctrl_c()
trap ctrl_c INT

function ctrl_c() {
	keepAsking=1
	while [ $keepAsking -eq 1 ]; do
		read -p "Do you wish to end this experiment? (answer y for yes) " yn
		case $yn in
			[Yy]* ) endExperiment; break;;
			* ) echo "You have selected no"; keepAsking=0;;
		esac
	done
}

function endExperiment() {
	echo " ====== ENDING EXPERIMENT ====== "
	
	#-- end program on robots
	./endProgram.sh
	
	#-- end observer program
	pkill roslaunch
	
	#-- copy results from observer
	cp ${OBSERVER_BIN}/infoEvents.txt ${RUN_RESULT_FILES_COMMON}infoEvents.txt
	#-- add results from robots
	for robotId in ${ROBOT_IDS[@]}; do
		./addRobotDataToFile.sh ${RUN_RESULT_FILES_COMMON}infoEvents.txt ${LOG_FILE_PREFIX} ${robotId} &
	done
	exit 0
}

while true; do
    sleep 1
done
