#!/bin/sh
wfb_soc=$(ipcinfo -c)
wfb_key=/etc/drone.key
wfb_dev=wlan0

wfb_yaml() {
	if [ -e /etc/wfb.yaml ]; then
		if [ "$1" = "b" ]; then
			config=broadcast
		elif [ "$1" = "t" ]; then
			config=telemetry
		elif [ "$1" = "w" ]; then
			config=wireless
		fi
		value=$(wfb-cli -g ".$config.$2")
	fi

	eval "$1_$2=${value:-$3}"
}

load_config() {
	wfb_yaml w txpower 1
	wfb_yaml w region 00
	wfb_yaml w channel 161
	wfb_yaml w mode HT20

	wfb_yaml b index 1
	wfb_yaml b fec_k 8
	wfb_yaml b fec_n 12
	wfb_yaml b link_id 7669206

	wfb_yaml t index 1
	wfb_yaml t router msposd
	wfb_yaml t serial /dev/ttyS2
	wfb_yaml t osd_fps 20
	wfb_yaml t port_rx 14551
	wfb_yaml t port_tx 14555
}

load_modules() {
	for card in $(lsusb | awk '{print $6}' | sort | uniq); do
		case "$card" in
			"0bda:8812" | "0bda:881a" | "0b05:17d2" | "2357:0101" | "2604:0012")
				driver=88XXau
				modprobe "$driver"
				;;

			"0bda:a81a")
				driver=8812eu
				modprobe "$driver" rtw_regd_src=1 rtw_tx_pwr_by_rate=0 rtw_tx_pwr_lmt_enable=0
				;;

			"0bda:f72b" | "0bda:b733")
				driver=8733bu
				modprobe "$driver" rtw_regd_src=1 rtw_tx_pwr_by_rate=0 rtw_tx_pwr_lmt_enable=0
				;;
		esac
	done

	if [ -z "$driver" ]; then
		echo "- Wireless module not detected!"
		exit 1
	else
		echo "- Detected driver: $driver"
	fi

	if ! ifconfig "$wfb_dev" up; then
		echo "- Wireless driver not found!"
		exit 1
	fi
}

load_interface() {
	iw "$wfb_dev" set monitor none
	iw "$wfb_dev" set channel "$w_channel" "$w_mode"
	iw reg set "$w_region"

	if [ "$driver" = "88XXau" ]; then
		iw "$wfb_dev" set txpower fixed $((w_txpower * -100))
	else
		iw "$wfb_dev" set txpower fixed $((w_txpower * 50))
	fi

	case "$w_mode" in
		"HT20")
			bandwidth=20
			;;
		"HT40+" | "HT40-")
			bandwidth=40
			;;
		"80Mhz")
			bandwidth=80
			;;
	esac
}

start_wfb() {
	echo "- Starting wfb_tx"
	wfb_tx -K "$wfb_key" -M "$b_index" -B "$bandwidth" -C 8000 \
		-k "$b_fec_k" -n "$b_fec_n" -i "$b_link_id" "$wfb_dev" &> /dev/null &
}

start_tunnel() {
	echo "- Starting wfb_tun"
	wfb_rx -p 160 -u 5800 -K "$wfb_key" -i "$b_link_id" "$wfb_dev" &> /dev/null &
	wfb_tx -p 32 -u 5801 -K "$wfb_key" -M "$t_index" \
		-k "$b_fec_k" -n "$b_fec_n" -i "$b_link_id" "$wfb_dev" &> /dev/null &
	wfb_tun -a 10.5.0.10/24 > /dev/null &
}

start_telemetry() {
	if [ "$wfb_soc" = "gk7205v200" ]; then
		devmem 0x120C0010 32 0x1E04
	elif [ "$wfb_soc" = "ssc33x" ]; then
		devmem 0x1F207890 16 0x8
	fi

	if [ "$t_router" = "msposd" ]; then
		echo "- Starting $t_router"
		msposd --baudrate 115200 ---channels 8 -osd --ahi 0 -r "$t_osd_fps" \
			--master "$t_serial" --out 10.5.0.1:"$t_port_tx" > /dev/null &
	elif [ "$t_router" = "mavfwd" ]; then
		echo "- Starting $t_router"
		mavfwd --baudrate 115200 ---channels 8 -p 100 -a 15 -t \
			--master "$t_serial" --in 0.0.0.0:"$t_port_rx" --out 10.5.0.1:"$t_port_tx" > /dev/null &
	fi
}

majestic_sigmastar() {
	cli -s .isp.sensorConfig /etc/sensors/imx415_fpv.bin
	cli -s .isp.exposure 5
	cli -s .video0.codec h265
	cli -s .video0.size 1920x1080
	cli -s .video0.fps 90
	cli -s .video0.rcMode cbr
	cli -s .jpeg.enabled false
	cli -s .outgoing.enabled true
	cli -s .outgoing.server udp://0.0.0.0:5600
}

case "$1" in
	start)
		if [ ! -e /etc/system.ok ]; then
			if [ "$wfb_soc" = "ssc33x" ]; then
				majestic_sigmastar
			fi

			touch /etc/system.ok
		fi

		load_config
		load_modules
		load_interface

		start_wfb
		start_tunnel
		start_telemetry
		;;

	stop)
		killall -q wfb_rx
		killall -q wfb_tx
		killall -q wfb_tun
		killall -q msposd
		killall -q mavfwd
		;;

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