#!/bin/bash

CONFIG_FILE="/etc/modalai/voxl-px4.conf"

AIRFRAME=MULTICOPTER
GPS=NONE
RC=SPEKTRUM
ESC=VOXL_ESC
POWER_MANAGER=VOXLPM
AIRSPEED_SENSOR=NONE
DISTANCE_SENSOR=NONE
OSD=DISABLE
DAEMON_MODE=DISABLE
SENSOR_CAL=ACTUAL
EXTRA_STEPS=()

# Try to source configuration variables from a file.
# This will override anything already set in the environment.
if [ -f "$CONFIG_FILE" ]; then
    echo "[INFO] Reading from $CONFIG_FILE"
    source $CONFIG_FILE
else
    echo "[INFO] $CONFIG_FILE not found, using defaults"
fi

# Make sure that the SLPI DSP test signature is there otherwise px4 cannot run
# on the DSP
if /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then
    /bin/echo "Found DSP signature file"
else
    /bin/echo "[WARNING] Could not find DSP signature file"
    # Look for the DSP signature generation script
    if [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then
        /bin/echo "[INFO] Attempting to generate the DSP signature file"
        # Automatically generate the test signature so that px4 can run on SLPI DSP
        /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh
    else
        /bin/echo "[ERROR] Could not find the DSP signature file generation script"
        exit 0
    fi
fi

print_usage() {
    echo -e "\nUsage: voxl-px4 [-a (Specify Airspeed Sensor)]"
    echo "                [-b (Specify Holybro GPS unit)]"
    echo "                [-c delete configuration file and exit]"
    echo "                [-d start px4 without daemon mode]"
    echo "                [-f (Use fake rc input instead of from a real transmitter)]"
    echo "                [-m (Specify Matek GPS unit)]"
    echo "                [-o (Start OSD module on the apps processor)]"
    echo "                [-p (Specify Fixed Wing airframe selected)]"
    echo "                [-r (Specify TBS Crossfire RC receiver, MAVLINK)]"
    echo "                [-w (Specify TBS Crossfire RC receiver, raw)]"
    echo "                [-z (Use fake sensor calibration values)]"
    echo "                [-h (show help)]"

    exit 1
}

print_config_settings(){
    echo -e "\n*************************"
    echo "AIRFRAME=$AIRFRAME"
    echo "GPS=$GPS"
    echo "RC=$RC"
    echo "ESC=$ESC"
    echo "POWER MANAGER=$POWER_MANAGER"
    echo "AIRSPEED SENSOR=$AIRSPEED_SENSOR"
    echo "DISTANCE SENSOR=$DISTANCE_SENSOR"
    echo "OSD=$OSD"
    echo "DAEMON_MODE=$DAEMON_MODE"
    echo "SENSOR_CAL=$SENSOR_CAL"
    echo "EXTRA STEPS:"
    for i in "${EXTRA_STEPS[@]}"
    do
        echo -e "\t$i"
    done
    echo -e "*************************\n"
}

while getopts "abcdhfmoprwz" flag
do
    case "${flag}" in
        a)
            echo "[INFO] MRO AIRSPEED Sensor selected"
            AIRSPEED_SENSOR=MS4525DO
            ;;
        b)
            echo "[INFO] Holybro GPS selected"
            GPS=HOLYBRO
            ;;
        c)
            echo "[INFO] Wiping old config file"
            if [ -f "$CONFIG_FILE" ]; then
                rm -rf ${CONFIG_FILE}
            fi
            exit 0
            ;;
        d)
            echo "[INFO] Disabling daemon mode"
            DAEMON_MODE=DISABLE
            ;;
        h)
            print_usage
            ;;
        f)
            echo "[INFO] Setting RC to FAKE_RC_INPUT"
            RC=FAKE_RC_INPUT
            ;;
        m)
            echo "[INFO] Matek GPS selected"
            GPS=MATEK
            ;;
        o)
            echo "[INFO] OSD module selected"
            OSD=ENABLE
            ;;
        p)
            echo "[INFO] Airframe Selected as Fixed Wing"
            AIRFRAME=FIXED_WING
            ;;
        r)
            echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected"
            RC=CRSF_MAV
            ;;
        w)
            echo "[INFO] TBS Crossfire RC receiver, raw selected"
            RC=CRSF_RAW
            ;;
        z)
            echo "[INFO] Fake sensor calibration values selected"
            SENSOR_CAL=FAKE
            ;;
        *)
            print_usage
            ;;
    esac
done

if [ $DAEMON_MODE == "DISABLE" ]; then
    DAEMON=" "
else
    echo "[INFO] Daemon mode enabled"
    DAEMON="-d"
fi

if [ $SENSOR_CAL == "FAKE" ]; then
    /bin/echo "[INFO] Setting up fake sensor calibration values"
    px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
    /bin/sync
fi

print_config_settings

AIRFRAME=$AIRFRAME GPS=$GPS RC=$RC ESC=$ESC POWER_MANAGER=$POWER_MANAGER DISTANCE_SENSOR=$DISTANCE_SENSOR \
AIRSPEED_SENSOR=$AIRSPEED_SENSOR OSD=$OSD EXTRA_STEPS=$EXTRA_STEPS \
px4 $DAEMON -s /usr/bin/voxl-px4-start