#!/bin/sh
fw=$(grep "BUILD_OPTION" "/etc/os-release" | cut -d= -f2)
keydir=/etc

if [ -e /etc/datalink.conf ]; then
	. /etc/datalink.conf
fi

if [ -e /etc/telemetry.conf ]; then
	. /etc/telemetry.conf
fi

if [ ! -e /usr/bin/telemetry_rx ] || [ ! -e /usr/bin/telemetry_tx ]; then
	ln -fs /usr/bin/wfb_rx /usr/bin/telemetry_rx
	ln -fs /usr/bin/wfb_tx /usr/bin/telemetry_tx
fi

start_drone_telemetry() {
	if [ "$router" -lt 2  ]; then
		if [ "$one_way" = "false" ]; then
			telemetry_rx -p "$stream_rx" -u "$port_rx" -K "$keydir/$unit.key" -i "$link_id" "$wlan" > /dev/null &
		fi
		telemetry_tx -p "$stream_tx" -u "$port_tx" -K "$keydir/$unit.key" -B "$bandwidth" \
			-M "$mcs_index" -S "$stbc" -L "$ldpc" -G "$guard_interval" -k "$fec_k" -n "$fec_n" \
			-T "$pool_timeout" -i "$link_id" -f "$frame_type" "$wlan" > /dev/null &
	fi
}

start_gs_telemetry() {
	if [ "$one_way" = "false" ]; then
		telemetry_tx -p "$stream_tx" -u "$port_tx" -K "$keydir/$unit.key" -B "$bandwidth" \
			-M "$mcs_index" -S "$stbc" -L "$ldpc" -G "$guard_interval" -k "$fec_k" -n "$fec_n" \
			-T "$pool_timeout" -i "$link_id" -f "$frame_type" "$wlan" > /dev/null &
	fi
	telemetry_rx -p "$stream_rx" -u "$port_rx" -K "$keydir/$unit.key" -i "$link_id" "$wlan" > /dev/null &
}

case "$1" in
	start)
		echo "Loading MAVLink telemetry service..."
		if [ "$router" -eq 1 ] || [ "$fw" = "lte" ]; then
			mavlink-routerd -c /etc/mavlink.conf > /dev/null 2>&1 &
		else
			if [ "$router" -eq 2  ]; then
				msposd --channels "$channels" --master "$serial" --baudrate "$baud"  \
					--out 127.0.0.1:$(($port_tx + 1)) -osd -r "$fps" --ahi "$ahi" > /dev/null &
			else
				mavfwd --channels "$channels" --master "$serial" --baudrate "$baud" -p 100 -t -a "$aggregate" \
					--out 127.0.0.1:$port_tx --in 127.0.0.1:$port_rx > /dev/null &
			fi
		fi
	
		if [ "$fw" = "fpv" ] || [ "$fw" = "venc" ]; then
			start_${unit}_telemetry
		fi
		;;

	stop)
		echo "Stopping telemetry services..."
		killall -q telemetry_rx
		killall -q telemetry_tx
		killall -q mavlink-routerd
		killall -q mavfwd
		killall -q msposd
		;;

	*)
		echo "Usage: $0 {start|stop}"
		exit 1
		;;
esac