#!/bin/sh
wfb_key=/etc/drone.key
wfb_cfg=/etc/wfb.yaml
wfb_dev=wlan0

echo_log() {
        echo "$1" | logger -s -t wifibroadcast
}

wfb_yaml() {
        if [ -e "$1" ]; then
                while IFS=: read -r key value; do
                        key=$(eval echo "$key")
                        value=$(eval echo "$value")
                        export "$key=$value"
                done < "$1"
        fi
}

load_config() {
        wfb_yaml /rom"$wfb_cfg"
        wfb_yaml "$wfb_cfg"
        [ ! -e "$wfb_key" ] && wfb_key=/rom"$wfb_key"
}

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

                        "0bda:a81a")
                                driver=8812eu
                                adapter_id="bl-m8812eu2"
                                ;;

                        "0bda:f72b" | "0bda:b733")
                                driver=8733bu
                                adapter_id="bl-m8731bu4"
                                ;;
                esac
        done

        if [ -z "$driver" ]; then
                echo_log "Wireless module not detected!"
                exit 1
        fi

        echo_log "Detected driver: $driver"
        [ -e /sys/class/net/"$wfb_dev" ] && return
        sleep 1

        if [ "$driver" != "88XXau" ]; then
                opt1="rtw_tx_pwr_by_rate=0"
                opt2="rtw_tx_pwr_lmt_enable=0"
        fi

        modprobe "$driver" "$opt1" "$opt2"
        sleep 3

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

        # Set .wireless.wlan_adapter in /etc/wfb.yaml if adapter_id is set
        if [ -n "$adapter_id" ]; then
                yaml-cli-multi -i /etc/wfb.yaml -s .wireless.wlan_adapter "$adapter_id"
        fi
}

load_mlink() {
        ifconfig "$wfb_dev" mtu "$mlink"

        nalu=$((mlink - 100))
        if [ "$(cli -g .outgoing.naluSize)" != "$nalu" ]; then
                cli -s .outgoing.naluSize "$nalu"
                killall -1 majestic
        fi
}

load_interface() {
        iw "$wfb_dev" set monitor none

        [ "$width" = "40" ] && wfb_width=HT40+ || wfb_width=HT20
        iw "$wfb_dev" set channel "$channel" "$wfb_width"
        iw reg set 00

        [ "$driver" = "88XXau" ] && wfb_power=$((txpower * -100)) || wfb_power=$((txpower * 50))
        iw "$wfb_dev" set txpower fixed "$wfb_power"
}

load_wfb() {
        wfb_rx -K "$wfb_key" -i "$link_id" -p "$1" -u "$3" "$wfb_dev" &> /dev/null &
        wfb_tx -K "$wfb_key" -M "$tun_index" -B "$width" -k "$fec_k" -n "$fec_n" \
                -S "$stbc" -L "$ldpc" -i "$link_id" -p "$2" -u "$4" "$wfb_dev" &> /dev/null &
}

start_broadcast() {
        echo_log "Starting wfb_tx"
        wfb_tx -K "$wfb_key" -M "$mcs_index" -B "$width" -k "$fec_k" -n "$fec_n" -U rtp_local \
                -S "$stbc" -L "$ldpc" -i "$link_id" -C 8000 -J 10 -E 5000 "$wfb_dev" &> /dev/null &
}

start_tunnel() {
        echo_log "Starting wfb_tun"
        load_wfb 160 32 5800 5801
        wfb_tun -a 10.5.0.10/24 > /dev/null &
}

start_telemetry() {
        chipset=$(ipcinfo -c)
        [ "$chipset" = "ssc33x" ] && devmem 0x1F207890 16 0x8

        port_rx=14550
        port_tx=14551

        echo_log "Starting $router"
        load_wfb 144 16 "$port_rx" "$port_tx"

        if [ "$router" = "mavfwd" ]; then
                mavfwd -b 115200 -c 8 -p 100 -a 15 -t -m /dev/"$serial" \
                        -o 127.0.0.1:14552 -i 127.0.0.1:"$port_rx" > /dev/null &
                mavlink-routerd &
        else
                if [ "$router" = "msposd" ]; then
                        size=$(curl -s localhost/api/v1/config.json | jsonfilter -qe "@.video0.size")
                fi
                msposd -b 115200 -c 8 -r "$osd_fps" -m /dev/"$serial" \
                        -o 127.0.0.1:"$port_tx" -z "$size" > /dev/null &

                sleep 5
                echo "&L70&F28&G8CPU:&C TEMP:&T &B" >/tmp/MSPOSD.msg
        fi
}

video_settings() {
        for card in $(lsusb | awk '{print $6}' | uniq); do
                case "$card" in
                        "0bda:8812" | "0bda:881a" | "0b05:17d2" | "2357:0101" | "2604:0012")
                                driver=88XXau
                                wifibroadcast cli -s .wireless.txpower 20
                                wifibroadcast cli -s .wireless.wlan_adapter bl-r8812af1
                                wifibroadcast cli -s .broadcast.stbc 1
                                wifibroadcast cli -s .broadcast.ldpc 1
                                ;;

                        "0bda:a81a")
                                driver=8812eu
                                wifibroadcast cli -s .wireless.txpower 40
                                wifibroadcast cli -s .wireless.wlan_adapter bl-m8812eu2
                                wifibroadcast cli -s .broadcast.stbc 1
                                wifibroadcast cli -s .broadcast.ldpc 1
                                ;;

                        "0bda:f72b" | "0bda:b733")
                                driver=8733bu
                                wifibroadcast cli -s .wireless.txpower 20
                                wifibroadcast cli -s .wireless.wlan_adapter bl-m8731bu4
                                wifibroadcast cli -s .broadcast.stbc 0
                                wifibroadcast cli -s .broadcast.ldpc 0
                                ;;
                esac
        done

        sensor=$(ipcinfo -s)
        family=$(ipcinfo -f)

        cli -s .isp.sensorConfig /etc/sensors/"$sensor"_"$family".bin
        cli -s .isp.exposure 16
        cli -s .video0.fps 60
        cli -s .video0.bitrate 8000
        cli -s .video0.codec h265
        cli -s .video0.rcMode cbr
        cli -s .outgoing.enabled true
        cli -s .outgoing.wfb true
        cli -s .records.split 1
        cli -s .records.notime true
        cli -s .fpv.enabled true
        cli -s .fpv.noiseLevel 0

        wifibroadcast cli -s .wireless.mlink 3994
        wifibroadcast cli -s .wireless.link_control alink
        sed -i '/alink_drone &/d' /etc/rc.local && sed -i -e '$i alink_drone &' /etc/rc.local

        if [ "$sensor" = "imx335" ]; then
                cli -s .video0.size 1920x1440
        else
                cli -s .video0.size 1920x1080
        fi
}

start() {
        load_config
        load_modules
        load_mlink
        load_interface

        if [ ! -e /etc/system.ok ]; then
                video_settings
                touch /etc/system.ok
                killall -1 majestic
        fi

        stop
        start_broadcast
        start_tunnel
        start_telemetry
}

stop() {
        for process in wfb_rx wfb_tx wfb_tun msposd mavfwd mavlink-routerd; do
                if [ -n "$(pidof $process)" ]; then
                        echo_log "Stopping $process"
                        killall -q "$process"
                fi
        done
}

case "$1" in
        start|stop)
                $1
                ;;

        reset)
                cp -f /rom"$wfb_cfg" "$wfb_cfg"
                cp -f /rom/etc/majestic.yaml /etc/majestic.yaml
                video_settings
                ;;

        cli)
                shift
                yaml-cli -i "$wfb_cfg" "$@"
                ;;

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