#!/usr/bin/python3
import os
import json
import argparse
import subprocess
import shlex

# COLORS!
RESET = "\033[0m"
GRN = "\033[1;32m"
LIT_GRN = "\033[92m"
RED = "\033[1;31m"
WARN = "\033[33m"
CLEAR ="\033c"
BOLD="\033[1m"
RESET_BOLD="\033[2m"

VTX_SERVICE_NAME="voxl-vtx"
CAMERA_SERVICE_NAME="voxl-camera-server"

VTX_CONFIG_FILE = "/etc/modalai/voxl-vtx.conf"
CAMERA_CONFIG_FILE = "/etc/modalai/voxl-camera-server.conf"

freq_to_ch = {
    2412: 1,
    2417: 2,
    2422: 3,
    2427: 4,
    2432: 5,
    2437: 6,
    2442: 7,
    2447: 8,
    2452: 9,
    2457: 10,
    2462: 11,
    2467: 12,
    2472: 13,
    2484: 14,
    5160: 32,
    5180: 36,
    5200: 40,
    5220: 44,
    5240: 48,
    5260: 52,
    5280: 56,
    5300: 60,
    5320: 64,
    5340: 68,
    5500: 100,
    5520: 104,
    5540: 108,
    5560: 112,
    5580: 116,
    5600: 120,
    5620: 124,
    5640: 128,
    5660: 132,
    5680: 136,
    5700: 140,
    5720: 144,
    5745: 149,
    5765: 153,
    5785: 157,
    5805: 161,
    5825: 165,
    5845: 169,
    5865: 173,
    5885: 177,
}
valid_bw = { 20, 40 }

H264_MAX_PAYLOAD = 1342
H265_MAX_PAYLOAD = 1341
DEFAULT_PAYLOAD  = 1000
LONG_RANGE_PAYLOAD = 750

# Define standard profiles
DEFAULT_PROFILE = {
    "name": "Default",
    "mcs": 0,
    "mtu": DEFAULT_PAYLOAD,
    "power": 1000,
    "source": "hires_default_misp_encoded",
    "invert_toggle":   -1,
    "joystick_toggle": -1,
    "ai_detection_enable": True,
    "ai_detection_source": "tflite_data",
    "ai_detection_qty": 3,
    "ai_detection_threshold": 0.5
}

TRACKING_FRONT_DEFAULT_CONFIG = {
    "name": "Tracking Front",
    "mcs": 0,
    "mtu": DEFAULT_PAYLOAD,
    "power": 1000,
    "source": "tracking_front_misp_encoded",
    "invert_toggle":   -1,
    "joystick_toggle": -1,
    "ai_detection_enable": False,
    "ai_detection_source": "tflite_data",
    "ai_detection_qty": 3,
    "ai_detection_threshold": 0.75
}

LONG_RANGE_PROFILE = {
    "name": "Long Range",
    "mcs": 0,
    "mtu": LONG_RANGE_PAYLOAD,
    "power": 1000,
    "source": "hires_long_range_misp_encoded",
    "invert_toggle":   -1,
    "joystick_toggle": -1,
    "ai_detection_enable": True,
    "ai_detection_source": "tflite_data",
    "ai_detection_qty": 3,
    "ai_detection_threshold": 0.5
}

LOW_LATENCY_PROFILE = {
    "name": "Low Latency",
    "mcs": 3,
    "mtu": H265_MAX_PAYLOAD,
    "power": 750,
    "source": "hires_low_latency_misp_encoded",
    "invert_toggle":   -1,
    "joystick_toggle": -1,
    "ai_detection_enable": True,
    "ai_detection_source": "tflite_data",
    "ai_detection_qty": 3,
    "ai_detection_threshold": 0.5
}

# Boson config
BOSON_DEFAULT_CONFIG:dict = {
    "name":     "Boson",
    "mcs":      0,
    "mtu":      H265_MAX_PAYLOAD,
    "power":    1000,
    "source":   "boson_misp_encoded",
    "invert_toggle":   12,
    "joystick_toggle":   -1,
    "ai_detection_enable": False,
    "ai_detection_source": "tflite_data",
    "ai_detection_qty":   3,
    "ai_detection_threshold": 0.75
}

# Downward Tracking config
DOWNWARD_TRACKING_DEFAULT_CONFIG:dict = {
    "name":     "Tracking Down",
    "mcs":      0,
    "mtu":      DEFAULT_PAYLOAD,
    "power":    1000,
    "source":   "tracking_down_misp_encoded",
    "invert_toggle":   -1,
    "joystick_toggle":   -1,
    "ai_detection_enable": False,
    "ai_detection_source": "tflite_data",
    "ai_detection_qty":   3,
    "ai_detection_threshold": 0.75
}

config_header = """/**
 * voxl-vtx Configuration File
 *
 * ============================================================================
 * WIRELESS TRANSMISSION SETTINGS
 * ============================================================================
 * frequency: Center frequency in MHz (VRX must match)
 * bandwidth: Channel bandwidth in MHz (20 or 40, VRX must match)
 * card: WiFi interface name (e.g., "wlan0")
 * enable_pit: Enable PIT mode (low transmit power for bench testing)
 *
 * ============================================================================
 * FORWARD ERROR CORRECTION
 * ============================================================================
 * enable_fec: Enable Forward Error Correction (VRX must match, True/False)
 * fec_percent: FEC overhead percentage [-1, 1000]
 *              -1: Auto calculated FEC %, minimum of 75%
 *               0: No FEC applied but headers included in payload
 *               >0: Specific percentage of FEC to use (Ex: 20, 75, 100)
 * fec_block_size: Number of fragments per FEC block (larger will be more computationally expensive)
 *
 * ============================================================================
 * ADVANCED WIRELESS SETTINGS
 * ============================================================================
 * enable_stbc: Enable Space-Time Block Coding (number of spatial streams)
 * regulatory_mode: Regulatory compliance mode ("off", "fcc")
 *
 * ============================================================================
 * VIDEO PROFILE CONFIGURATION
 * ============================================================================
 * active_profile_idx: Index of currently active video profile (0-based)
 * rc_toggle: RC channel to use for profile switching
 * osd_rc_toggle: RC channel for OSD enable/disable control (-1=disabled, 1-18=channel, LOW=osd enabled, HIGH=osd disabled)
 * osd_joystick_toggle: Joystick button bit for OSD enable/disable (-1=disabled, 0-32=bit, LOW=osd enabled, HIGH=osd disabled)
 * profiles: Array of video stream profiles (see profile section below)
 *
 * Profile Configuration:
 *   name: Profile name (Default, Boson, etc)
 *   source: Pipe name for video input (ex: "hires_default_misp_encoded")
 *   mcs: Modulation and Coding Scheme (0-7, higher=faster but less robust)
 *   mtu: Image payload size in bytes
 *   power: Transmit power in mW (0-1000)
 *   invert_toggle: RC channel to use for camera color invert (Boson only, white/black hot)
 *   joystick_toggle: Button bit to use for joystick control
 *   ai_detection_enable: Enable AI object detection for this profile
 *   ai_detection_source: Source pipe for object detection data for this profile 
 *   ai_detection_qty: Maximum number of detected objects to pass to VRX
 *   ai_detection_threshold: Detection confidence threshold (0.0-1.0)
 *
 * ============================================================================
 * TELEMETRY & DATA RATES
 * ============================================================================
 * info_rate_hz: Rate in hertz at which VTX info packets are sent to VRX
 * mavlink_telem_rate_hz: Rate in hertz at which MAVLink telemetry is forwarded to VRX
 * team_race_en: Enable team race behavior 
 *            "off": Disabled
 *            "on":  Enabled 
 *            "auto": Attempt to auto detect from ELRS receiver settings
 * team_race_tx_timeout_ms: Amount of time in milliseconds to TX for after ELRS failsafe
 *
 * ============================================================================
 * UDP MODE (for LTE/Doodle/Silvus/etc)
 * ============================================================================
 * udp: Use UDP for TX/RX instead of packet injection
 * udp_interface_ip: Local IP address for UDP interface
 * udp_interface_tx_port: Local transmit port for UDP (-1 to disable)
 * udp_interface_rx_port: Local receive port for UDP
 * udp_receiver_configs: Array of remote UDP receivers to send to
 *   ip: IP address of remote endpoint
 *   port: Port of remote endpoint
 *
 * ============================================================================
 * HARDWARE & SYSTEM SETTINGS
 * ============================================================================
 * i2c_port: I2C bus for temperature/power monitoring
 *           -1: Disabled
 *           0+: Enabled, Autodetect VTX power and temperature sensor
 * perf_mode: Set CPU to performance governor when armed for lower latency (True/False)
 *
 * ============================================================================
 * DEBUG & TESTING
 * ============================================================================
 * disable_transmissions: Disable wireless transmissions (for testing/debug)
 * debug_log: Debug logging to disk behavior
 *            "off": No logging enabled
 *            "on":  Start logging on application startup
 *            "arm": Start logging when drone is armed
 *
 */
 """

default_config = {
    # WIRELESS TRANSMISSION SETTINGS
    "frequency": 5805,
    "bandwidth": 20,
    "card": "wlan0",
    "enable_pit": True,

    # FORWARD ERROR CORRECTION
    "enable_fec": True,
    "fec_percent": -1,
    "fec_block_size": 30,

    # ADVANCED WIRELESS SETTINGS
    "enable_stbc": 1,
    "regulatory_mode": "off",

    # VIDEO PROFILE CONFIGURATION
    "active_profile_idx": 0,
    "rc_toggle": -1,
    "osd_rc_toggle": -1,
    "osd_joystick_toggle": -1,
    "profiles": [],

    # TELEMETRY & DATA RATES
    "info_rate_hz": 5,
    "mavlink_telem_rate_hz": 30,
    "team_race_en": "off",
    "team_race_tx_timeout_ms": 0,

    # UDP MODE
    "udp": False,
    "udp_interface_ip": "",
    "udp_interface_tx_port": -1,
    "udp_interface_rx_port": 50001,
    "udp_receiver_configs": [
        {
            "ip": "",
            "port": -1
        }
    ],

    # HARDWARE & SYSTEM SETTINGS
    "i2c_port": 0,  # Always enable this
    "perf_mode": False,

    # DEBUG & TESTING
    "disable_transmissions": False,
    "debug_log": "off"
}

# Parse args
def parse_args() -> dict:
    """
    Parse and validate args
    """
    parser = argparse.ArgumentParser(add_help=False)
    parser.add_argument('-h', '--help',        help='Show this help message and exit', required=False, action='store_true')
    parser.add_argument('-l', '--list',        help='Show the current configuration',  required=False, action='store_true')

    # Wifibroadcast args
    parser.add_argument('-F', '--frequency',   help='Set frequency', required=False, type=int)
    parser.add_argument('-a', '--active_profile_idx',  help='Set Active Profile', required=False, type=int)

    # Profile specific args
    parser.add_argument('-m', '--mcs',         help='Set MCS index', required=False, type=int)
    parser.add_argument('-s', '--source',      help='Set pipe to get video from', required=False, type=str)
    parser.add_argument('-p', '--power',       help='Set transmitter power (idx)', required=False, type=int)
    parser.add_argument('-P', '--power-mw',    help='Set transmitter power in mW', required=False, type=int)

    # Switch between voxl-vtx and voxl-softap and factory settings for voxl-vtx
    parser.add_argument('-t', '--toggle',               help='Toggle between voxl-vtx and voxl-softap',   required=False, action='store_true')
    parser.add_argument('-e', '--enable',               help='Load modified kernel module for RTL8812AU', required=False, action='store_true')
    parser.add_argument('-d', '--disable',              help='Load default kernel module for RTL8812AU',  required=False, action='store_true')
    parser.add_argument('-f', '--factory_enable',       help='Load modified kernel module for RTL8812AU and restore default configuration file', required=False, action='store_true')
    parser.add_argument('-L', '--factory_enable_lte',   help='Setup for LTE operation', required=False, action='store_true')
    parser.add_argument('-I', '--factory_enable_fiber', help='Setup for fiber operation', required=False, action='store_true')
    parser.add_argument('-D', '--factory_enable_doodle',    help='Setup for Doodle operation', required=False, action='store_true')
    parser.add_argument('--factory_enable_dtc',       help='Setup for DTC operation', required=False, action='store_true')
 
    # SKU specific setups
    parser.add_argument('--d0008_v4',             help='Setup for D0008_v4', required=False, action='store_true')
    parser.add_argument('--d0008_v5',             help='Setup for D0008_v5', required=False, action='store_true')
    parser.add_argument('--d0013',                help='Setup for D0013',    required=False, action='store_true')
    parser.add_argument('--d0020_strike',         help='Setup for D0020 strike', required=False, action='store_true')

    # Wizard
    parser.add_argument('-w', '--wizard',     help='Start interactive wizard', required=False, action='store_true')

    return vars(parser.parse_args())

# Print help message
def print_usage() -> None:
    """
    Print usage/help message
    """

    print("Description: Configure voxl-vtx settings.")
    print("")
    print("Usage: voxl-configure-vtx -h -l -F -m -s -p -P -t -e -d -f -w")
    print("")
    print("Optional Arguments (for VTX settings only, besides the wizard):")
    print(" -h, --help                  Show this help message and exit")
    print(" -l, --list                  Show the current configuration")

    # Wifibroadcast args
    print(" -F, --frequency             Set frequency")
    print(" -m, --mcs                   Set MCS index for the current active profile")
    print(" -s, --source                Set pipe to get video from for the current active profile")
    print(" -p, --power                 Set power idx (not mW) for the current active profile (does not save to configuration file)")
    print(" -P, --power-mw              Set power in milliwatts (mW) for the current active profile")


    # Toggle wifibroadcast/softap args
    print(" -t, --toggle                    Toggle between Wifibroadcast and SoftAP mode")
    print(" -e, --enable                    Load modified kernel module for RTL8812AU and enable voxl-vtx service")
    print(" -d, --disable                   Load default kernel module for RTL8812AU and remove modified kernel module after stopping and disabling voxl-vtx service")
    print(" -f, --factory_enable            Load modified kernel module for RTL8812AU, set efuse, enable voxl-vtx service, and restore default configuration file")
    print(" -L, --factory_enable_lte        Setup for LTE operation")
    print(" -D, --factory_enable_doodle     Setup for Doodle radio")
    print(" --factory_enable_dtc            Setup for DTC radio")

    # SKU specific setups
    print("     --d0008_v4              Setup for d0008_v4")
    print("     --d0008_v5              Setup for d0008_v5")
    print("     --d0013                 Setup for d0013")
    print("     --d0020_strike          Setup for d0020 strike")

    # Wizard
    print(" -w, --wizard                Start interactive wizard")
    print("")

# Input validation helper
def validate_service_name(name: str) -> str:
    """
    Validate service name contains only safe characters.
    Raises ValueError if validation fails.
    """
    import re
    if not re.match(r'^[a-zA-Z0-9._-]+$', name):
        raise ValueError(f"Invalid service name: {name}")
    return name

# Check if a service is running
def service_running(service_name: str) -> bool:
    """
    Checks if a systemd service is running or not

    @service_name: name of service to check
    @return: True/False depending on service state
    """
    service_name = validate_service_name(service_name)
    result = subprocess.run(['systemctl', 'is-active', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode == 0:
        return True
    else:
        return False
    
# Check if a service is enabled
def service_enabled(service_name: str) -> bool:
    """
    Checks if a systemd service is enabled or not

    @service_name: name of service to check
    @return: True/False depending on service state
    """
    service_name = validate_service_name(service_name)
    result = subprocess.run(['systemctl', 'is-enabled', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode == 0:
        return True
    else:
        return False

# Restart a service
def restart_service(service_name: str) -> bool:
    """
    Restarts a systemd service

    @service_name: name of service to restart
    @return: True/False depending on if successful
    """
    service_name = validate_service_name(service_name)
    print(f"Restarting {service_name}, this may take a few seconds...")
    result = subprocess.run(['systemctl', 'restart', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode != 0:
        print(f"Failed to restart {service_name}.")
        return False

    return True

# Enable and start a service
def enable_and_start_service(service_name: str) -> bool:
    """
    Enables and starts a systemd service

    @service_name: name of service to enable and starts
    @return: True/False depending on if successful
    """
    service_name = validate_service_name(service_name)

    # Enable service
    print(f"Enabling {service_name} systemd service\n")
    result = subprocess.run(['systemctl', 'enable', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode != 0:
        print(f"Failed to enable {service_name}.")
        return False
    
    # Start service
    print(f"Starting {service_name} systemd service\n")
    result = subprocess.run(['systemctl', 'start', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode != 0:
        print(f"Failed to start {service_name}.")
        return False
    
    return True

# Disable and stop a service
def disable_and_stop_service(service_name: str) -> bool:
    """
    Disables and stops a systemd service

    @service_name: name of service to disable and stop
    @return: True/False depending on if successful
    """
    service_name = validate_service_name(service_name)

    # Disable service
    print(f"Disabling {service_name} systemd service\n")
    result = subprocess.run(['systemctl', 'disable', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode != 0:
        print(f"Failed to disable {service_name}.")
        return False
    
    # Stop service
    print(f"Stopping {service_name} systemd service\n")
    result = subprocess.run(['systemctl', 'stop', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode != 0:
        print(f"Failed to stop {service_name}.")
        return False
    
    return True

# Enable a service
def enable_service(service_name: str) -> bool:
    """
    Enables a systemd service

    @service_name: name of service to enable
    @return: True/False depending on if successful
    """
    service_name = validate_service_name(service_name)

    # Enable service
    print(f"Enabling voxl-vtx systemd service\n")
    result = subprocess.run(['systemctl', 'enable', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode != 0:
        print(f"Failed to enable {service_name}.")
        return False
    
    return True

# Disable a service
def disable_service(service_name: str) -> bool:
    """
    Disables a systemd service

    @service_name: name of service to disable
    @return: True/False depending on if successful
    """
    service_name = validate_service_name(service_name)

    # Disable service
    print(f"Disabling voxl-vtx systemd service\n")
    result = subprocess.run(['systemctl', 'disable', shlex.quote(service_name)], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode != 0:
        print(f"Failed to disable {service_name}.")
        return False
    
    return True

# Change between voxl-vtx and softap, they both use the same network interface so only one can run at a time
def toggle_vtx_softap() -> int:
    """
    Change between voxl-vtx and softap
    """
    if service_running("voxl-softap"):
        print("Currently in softap mode. Transitioning to vtx mode: disabling voxl-softap and enabling voxl-vtx\n")
        if not (disable_and_stop_service("voxl-softap") and enable_and_start_service("voxl-vtx")):
            print(f"{BOLD}{RED}Failed to transition to VTX mode{CLEAR}")
            return -1
    elif service_running("voxl-vtx"):        
        print("Currently in VTX mode. Transitioning to softap mode: disabling voxl-vtx and enabling voxl-softap\n")
        if not (disable_and_stop_service("voxl-vtx") and enable_and_start_service("voxl-softap")):
            print(f"{BOLD}{RED}Failed to transition to softap mode{CLEAR}")
            return -1
    else:
        print("Neither voxl-vtx or voxl-softap currently running!")
    return 0

# Helper to load JSON file with optional comments
def load_json_with_comments(file_path: str) -> dict:
    """
    Load JSON from a file, filtering out lines containing '*' (comment lines).

    @file_path: Path to the JSON file
    @return: Parsed JSON as a dictionary
    """
    with open(file_path, 'r') as f:
        filtered_lines = [line for line in f if '*' not in line]
        return json.loads(''.join(filtered_lines))

# Helper to save JSON file with header comments
def save_json_with_header(file_path: str, config: dict) -> None:
    """
    Save JSON to a file with header comments preserved.

    @file_path: Path to the JSON file
    @config: Dictionary to save as JSON
    """
    with open(file_path, 'w') as f:
        f.write(config_header)
        json.dump(config, f, indent=4)
        f.write('\n')

# Read a file
def read_file(file: str) -> None:
    """
    Read the contents of a file given a file name.
    """
    if not os.path.exists(file):
        print(f"{BOLD}{RED}[ERROR]{RESET_BOLD} File {file} does not exist!")
        return

    with open(file, 'r') as config:
        print(config.read().strip())

def write_factory_default_config() -> None:
    """
    Write the factory default config file.
    """
    save_json_with_header(VTX_CONFIG_FILE, default_config)

def write_default_config(d0008_v5: bool, d0013: bool, include_cameras: bool = False) -> None:
    """
    Write the default config file with ordered profiles based on available cameras.
    """
    print("VTX Configuration:")

    # Start with default config structure (make a copy)
    config = default_config.copy()
    config["profiles"] = []

    # Add default profile first
    config["profiles"].append(DEFAULT_PROFILE)

    # Check for additional camera-based profiles
    if d0008_v5 or d0013 or include_cameras:
        try:
            camera_config_json = load_json_with_comments(CAMERA_CONFIG_FILE)
            camera_names = {camera["name"].lower() for camera in camera_config_json["cameras"]}
        except Exception as e:
            print(f"{WARN}[WARNING] Could not parse camera config: {e}{RESET}")
            camera_names = set()
        
        has_boson = any("boson" in name for name in camera_names)

        if has_boson:
            config["profiles"].append(BOSON_DEFAULT_CONFIG)
            if "tracking_front" in camera_names:
                config["profiles"].append(TRACKING_FRONT_DEFAULT_CONFIG)
            if "tracking_down" in camera_names:
                config["profiles"].append(DOWNWARD_TRACKING_DEFAULT_CONFIG)
        else:
            if "tracking_front" in camera_names:
                config["profiles"].append(TRACKING_FRONT_DEFAULT_CONFIG)
            if "tracking_down" in camera_names:
                config["profiles"].append(DOWNWARD_TRACKING_DEFAULT_CONFIG)


    # Add universal fallback profiles
    config["profiles"].extend([LONG_RANGE_PROFILE, LOW_LATENCY_PROFILE])

    # Write config to file
    save_json_with_header(VTX_CONFIG_FILE, config)

    # Print for visual confirmation
    for profile in config["profiles"]:
        print(f"\tProfile: {profile['name']} - {profile['source']}")


# update config file via arg and val
def update_config_param(arg: str, value, parent: str = ""):
    """
    Update a param in the config file.
    @arg: The field to be updated
    @value: The value for the field to be set to 
    @parent: Nested/Subfield that the given arg is in 
    """
    # Ensure the config file exists
    if not os.path.exists(VTX_CONFIG_FILE):
        print(f"Config file '{VTX_CONFIG_FILE}' not found. Creating with default values.")
        save_json_with_header(VTX_CONFIG_FILE, default_config)

    # Read the existing config file
    try:
        config = load_json_with_comments(VTX_CONFIG_FILE)
    except json.JSONDecodeError as e:
        raise ValueError(f"Error decoding JSON from config file: {e}")

    # Update the configuration
    if not parent and arg in config:
        config[arg] = value
    elif parent and parent in config: # Nested value
        for list_item in config[parent]:
            if arg in list_item:
                list_item[arg] = value
    else:
        raise KeyError(f"Key '{arg}' not found in the configuration file.")

    # Write the updated config back to the file
    save_json_with_header(VTX_CONFIG_FILE, config)

    print(f"{arg} -> {value}")

# update config file via arg and val
def update_profile_param(arg, value, profile_name:str):
    """
    Update a profile param in the config file.
    @arg: The field to be updated
    @value: The value for the field to be set to 
    @profile_name: Profile to update 
    """
    # Ensure the config file exists
    if not os.path.exists(VTX_CONFIG_FILE):
        print(f"Config file '{VTX_CONFIG_FILE}' not found. Creating with default values.")
        save_json_with_header(VTX_CONFIG_FILE, default_config)

    # Read the existing config file
    try:
        config = load_json_with_comments(VTX_CONFIG_FILE)
    except json.JSONDecodeError as e:
        raise ValueError(f"Error decoding JSON from config file: {e}")

    # Update the configuration
    for profile in config["profiles"]:
        if profile_name == profile["name"]:
            profile[arg] = value

    # Write the updated config back to the file
    save_json_with_header(VTX_CONFIG_FILE, config)

    print(f"{arg} -> {value}")

# Dump the current configuration
def dump_config() -> None:
    """
    Show the current configuration.
    """
    vtx_config_json = load_json_with_comments(VTX_CONFIG_FILE)
    if vtx_config_json.get('mcs') is not None:                   print(f"MCS: {vtx_config_json['mcs']}")
    if vtx_config_json.get('power') is not None:                 print(f"Power: {vtx_config_json['power']}")
    if vtx_config_json.get('card') is not None:                  print(f"Card: {vtx_config_json['card']}")
    if vtx_config_json.get('bandwidth') is not None:             print(f"Bandwidth: {vtx_config_json['bandwidth']}")
    if vtx_config_json.get('frequency') is not None:             print(f"Frequency: {vtx_config_json['frequency']}")
    if vtx_config_json.get('enable_fec') is not None:            print(f"Enable_FEC: {vtx_config_json['enable_fec']}")
    if vtx_config_json.get('fec_percent') is not None:           print(f"FEC_percent: {vtx_config_json['fec_percent']}")
    if vtx_config_json.get('fec_block_size') is not None:        print(f"FEC_block_size: {vtx_config_json['fec_block_size']}")
    if vtx_config_json.get('enable_stbc') is not None:           print(f"Enable_stbc: {vtx_config_json['enable_stbc']}")
    if vtx_config_json.get('perf_mode') is not None:             print(f"Perf_mode: {vtx_config_json['perf_mode']}")
    if vtx_config_json.get('enable_pit') is not None:            print(f"Enable_pit: {vtx_config_json['enable_pit']}")
    if vtx_config_json.get('i2c_port') is not None:              print(f"I2C Port: {vtx_config_json['i2c_port']}")
    if vtx_config_json.get('rc_toggle') is not None:             print(f"Toggle: {vtx_config_json['rc_toggle']}")
    if vtx_config_json.get('source') is not None:                print(f"Source: {vtx_config_json['source']}")
    if vtx_config_json.get('secondary_source') is not None:      print(f"Secondary_source: {vtx_config_json['secondary_source']}")
    if vtx_config_json.get('disable_transmissions') is not None: print(f"Disable_transmissions: {vtx_config_json['disable_transmissions']}")

def reset_camera_configuration() -> None:
    """
    Run voxl-configure-cameras to reset the camera configuration to match the SKU 
    """
    inpsect_sku = subprocess.run(['voxl-inspect-sku', '--json'], stdout=subprocess.PIPE)
    sku_json = json.loads(inpsect_sku.stdout)

    if sku_json["camera_config"] is not None:
        subprocess.run(['voxl-configure-cameras', shlex.quote(sku_json['camera_config'])])
    else:
        print("Failed to find camera configuration")
    
# Load modified 88XXau .ko
def enable_vtx(): 
    """
    Swap default 88XXau kernel module with modified 88XXau kernel module for wfb
    """
    enable_service(VTX_SERVICE_NAME)
    subprocess.run(['bash', '/usr/bin/enable_vtx.sh'])

# Load modified 88XXau .ko
def factory_enable_vtx(args, restart=False) -> int: 
    """
    Swap default 88XXau kernel module with modified 88XXau kernel module for wfb
    """
    # Restore default config file
    write_default_config(args.get("d0008_v5", False) or args.get("d0020_strike", False), args.get("d0013", False))
    update_config_param("udp", False)
    enable_service(VTX_SERVICE_NAME)
    
    # Set the efuse before swapping kernel modules 
    print("Setting M0185 efuse")
    rc = subprocess.run(['bash', '/usr/bin/set_efuse.sh'], stdout=subprocess.DEVNULL)
    if(rc.returncode):
        print(f"{RED}{BOLD}[ERROR] Set efuse script failed. RC: {rc.returncode}{RESET}")
    ret = rc.returncode

    rc = subprocess.run(['bash', '/usr/bin/enable_vtx.sh'])
    if(rc.returncode): 
        print(f"{RED}{BOLD}[ERROR] Enable vtx script failed.{RESET}")

    ret |= rc.returncode

    # if restart: restart_service(VTX_SERVICE_NAME)
    return ret

# Setup for LTE operations
def factory_enable_lte(args) -> int:
    """
    Swap default 88XXau kernel module with modified 88XXau kernel module for wfb
    """
    # Restore default config file
    write_default_config(args.get("d0008_v5", False), args.get("d0013", False), include_cameras=True)
    update_config_param("udp", True)
    enable_service(VTX_SERVICE_NAME)
    # restart_service(VTX_SERVICE_NAME)

# Enable services and load default config file for SparkLAN modem
def factory_enable_doodle(args):
    """
    Enable voxl-vtx service and update config file for Doodle use case
    """
    write_default_config(args.get("d0008_v5", False), args.get("d0013", False), include_cameras=True)
    update_config_param("udp", True)
    update_config_param("udp_interface_ip", "10.223.0.100")
    update_config_param("ip", "10.223.0.101", "udp_receiver_configs")
    update_config_param("port", 50000, "udp_receiver_configs")
    enable_service(VTX_SERVICE_NAME)
    # restart_service(VTX_SERVICE_NAME)

# Enable services and load default config file for SparkLAN modem
def factory_enable_dtc(args):
    """
    Enable voxl-vtx service and update config file for DTC use case
    """
    write_default_config(args.get("d0008_v5", False), args.get("d0013", False), include_cameras=True)
    update_config_param("udp", True)
    update_config_param("udp_interface_ip", "192.168.0.100")
    update_config_param("ip", "192.168.0.101", "udp_receiver_configs")
    update_config_param("port", 50000, "udp_receiver_configs")
    enable_service(VTX_SERVICE_NAME)
    # restart_service(VTX_SERVICE_NAME)

# Enable services and load default config file for fiber configuration
def factory_enable_fiber(args):
    """
    Enable voxl-vtx service and update config file for fiber use case
    """
    write_default_config(True, args.get("d0013", False), include_cameras=True)
    update_config_param("udp", True)
    update_config_param("udp_interface_ip", "10.0.0.10")
    update_config_param("ip", "10.0.0.2", "udp_receiver_configs")
    update_config_param("port", 50000, "udp_receiver_configs")
    # only one camera for fiber use-case, disable camera switching to prevent accidental stream dropping
    # update_config_param("rc_toggle", 11)
    # update_profile_param("joystick_toggle", 9, "Default")
    # update_profile_param("joystick_toggle", 8, "Tracking Front")
    # update_profile_param("joystick_toggle", 7, "Tracking Down")
    enable_service(VTX_SERVICE_NAME)

# Load modified 88XXau .ko
def disable_vtx(): 
    """
    Swap modified 88XXau_vtx kernel module with default 88XXau kernel module 
    """
    disable_service(VTX_SERVICE_NAME)
    subprocess.run(['bash', '/usr/bin/disable_vtx.sh'])

# Verify power is within valid range
def verify_power(power: int) -> int:
    """
    Verify that the power setting a user is trying to apply is valid and truncate it to within a acceptable/safe range is invalid value is given

    @power: The power index that the user wants to set 
    @return: The validated or truncated power index 
    """
    if power > 53: print(f"{WARN}{BOLD}[WARNING] Setting TPI above 53 may damage your hardware.{RESET}")
    if power > 63:
        print(f"{WARN}{BOLD}[WARNING] TPI above max requested: {power}. Max TPI is 63, setting to max.{RESET}")
        power = 63
    return power 

# Verify power is within valid range (0-1000mw)
def verify_power_mw(power: int) -> int:
    """
    Verify that the power setting a user is trying to apply is valid and truncate it to within a acceptable/safe range is invalid value is given

    @power: The power index that the user wants to set 
    @return: The validated or truncated power index 
    """
    if power > 1000:
        print(f"{WARN}{BOLD}[WARNING] Max power output is 1000mW.{RESET}")
        power = 1000
    elif power < 0:
        print(f"{WARN}{BOLD}[WARNING] Minimum power output is 0mW.{RESET}")
        power = 0
    return power 

# Verify mcs is within valid range
def verify_mcs(mcs: int) -> int:
    """
    Verify that the mcs index a user is trying to apply is valid and truncate it to within a acceptable/safe range is invalid value is given

    @mcs: The mcs index that the user wants to set 
    @return: The validated or truncated mcs index 
    """
    if mcs > 7:
        print(f"{WARN}{BOLD}[WARNING] MCS index provided ({mcs}) is invalid, must be 0-7.{RESET}")
        return -1

    if mcs < 0:
        print(f"{WARN}{BOLD}[WARNING] MCS index provided ({mcs}) is invalid, must be 0-7.{RESET}")
        return -1

    return mcs 

# Set override value for frequency 
def set_override_frequency(frequency:int) -> None:
    """
    Set the override frequency in the rtl8812au kernel driver. This value will be set
    instead of whatever frequency is passed into the command "iw dev wlan0 set freq 5180 HT20"
    
    @frequency: Frequency to set the VTX to. Will default to 5865MHz if requested freq is not in dict
    @Return: None
    """
    print(f"Got frequency: {frequency}") 
    channel_file:str = "/sys/module/88XXau_vtx/parameters/channel_override"
    if not os.path.exists(channel_file): 
        print(f"{WARN}[WARNING] Channel override file is missing, please run --enable of --factory_enable to load the correct kernel module.{RESET}")
        return
    with open(channel_file, 'w') as override_file:
        if (override_file.writable()): override_file.write(f"{freq_to_ch.get(frequency, 173)}")
        else: print(f"Can't write to file: {override_file}")

# Update a VTX Profile
def update_vtx_profile() -> None:
    """
    Wizard for selecting and editing individual VTX profiles.
    """
    while True:
        # Load current config
        try:
            cfg = load_json_with_comments(VTX_CONFIG_FILE)
        except FileNotFoundError:
            print("[ERROR] Cannot find VTX config file.")
            return

        profiles = cfg.get('profiles', [])
        num = len(profiles)

        # Build menu
        print(f"{LIT_GRN}Select a profile to edit:{RESET}")
        print("[1] Quit")
        print("[2] Back")
        print("[3] Restart Service")
        for idx, p in enumerate(profiles):
            print(f"[{idx + 4}] {p.get('name','<unnamed>')}")

        choice = input()
        try:
            field = int(choice)
            print(f"{CLEAR}", end="")
        except KeyboardInterrupt:
            print()
            exit(0)
        except ValueError:
            print(f"{WARN} Invalid selection.{RESET}")
            continue

        # Handle special options
        if field == 1:
            exit(0)             
        if field == 2:
            return              # Back to top‐level wizard
        if field == 3:
            restart_service(VTX_SERVICE_NAME)
            continue

        # Handle profiles now
        prof_idx = field - 4
        if prof_idx < 0 or prof_idx >= num:
            print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")
            continue

        # Now edit a single profile
        while True:
            prof = cfg['profiles'][prof_idx]
            print(f"{LIT_GRN}Editing profile [{prof_idx}] {prof['name']}{RESET}")
            print("[1] Quit")
            print("[2] Back")
            print("[3] Restart Service")
            fields = ["name", "mcs", "power", "source"]
            for i, fld in enumerate(fields, start=4):
                print(f"[{i}] {fld} (current: {prof[fld]})")

            sub = input()
            try:
                profile_selected = int(sub)
                print(f"{CLEAR}", end="")
            except KeyboardInterrupt:
                print()
                exit(0)
            except ValueError:
                print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")
                continue

            if profile_selected == 1:
                exit(0)
            if profile_selected == 2:
                break    # back to profiles list
            if profile_selected == 3:
                restart_service(VTX_SERVICE_NAME)
                continue

            fld_idx = profile_selected - 4
            if fld_idx < 0 or fld_idx >= len(fields):
                print(f"{WARN} Selection out of range.{RESET}")
                continue

            field = fields[fld_idx]
            old = prof[field]

            # Prompt for the right type
            try:
                if field == "name" or field == "source":
                    new = input(f"Enter new {field} (current: {old}): ")
                elif field == "mcs":
                    val = int(input(f"Enter new mcs (0-7, current {old}): "))
                    if verify_mcs(val) == -1:
                        continue
                    new = val
                elif field == "power":
                    val = int(input(f"Enter new power in mW (current: {old}): "))
                    new = verify_power_mw(val)
                else:
                    continue
            except (KeyboardInterrupt, ValueError):
                    print(f"{WARN} Invalid input: {sub}:{field}:{old}.{RESET}")
                    continue

            # Save changes
            print(f"\t{field}: {old} -> {new}")
            cfg['profiles'][prof_idx][field] = new
            save_json_with_header(VTX_CONFIG_FILE, cfg)

# Handle updating wifibroadcast configuration file
def update_vtx_config(args: dict) -> int:
    """
    Update parameters in VTX configuration file given a set of args from command line
    @args: Dictionary of args passed in at the command line containing fields to be updated in the wifibroadcast configuration file
    """
    rc : int = 0
    try:
        if args.get("d0008_v4") or args.get("d0008_v5") or args.get("d0013") or args.get("d0020_strike"):
            rc = factory_enable_vtx(args)

        vtx_config_json = load_json_with_comments(VTX_CONFIG_FILE)

        # For MCS, Power, and Source we need to get the active profile idx first, validate user input
        active_profile_idx = vtx_config_json.get("active_profile_idx", 0)
        num_profiles = len(vtx_config_json.get('profiles', []))

        # Handle all the args passed in
        for field, value in args.items():
            if value is None or value is False: continue
            if field == "mcs":
                if verify_mcs(value)  == -1:
                    continue
                _ = subprocess.run(['voxl-send-command', 'vtx_latency', 'set_mcs', shlex.quote(str(value))], stdout=subprocess.DEVNULL)
            elif field == "power":
                value = verify_power(value)
                _ = subprocess.run(['voxl-send-command', 'vtx_latency', 'set_power', shlex.quote(str(value))], stdout=subprocess.DEVNULL)
                continue    # Don't write value to config file
            elif field == "power_mw":
                value = verify_power_mw(value)
                _ = subprocess.run(['voxl-send-command', 'vtx_latency', 'set_mw_power', shlex.quote(str(value))], stdout=subprocess.DEVNULL)
                field = "power"
            elif field == "frequency":
                if freq_to_ch.get(value, -1) == -1:
                    print(f"Invalid frequency provided: {value}.")
                    continue
                set_override_frequency(value)
                _ = subprocess.run(['voxl-send-command', 'vtx_latency', 'set_frequency', shlex.quote(str(value))], stdout=subprocess.DEVNULL)
            elif field == "active_profile_idx":
                if value+1 > num_profiles:
                    print(f"Provided active profile idx ({value}, zero indexed) is not valid. Number of provided profiles: {num_profiles}")
                    continue
                _ = subprocess.run(['voxl-send-command', 'vtx_latency', 'set_profile_idx', shlex.quote(str(value))], stdout=subprocess.DEVNULL)
            elif field == "d0008_v4":
                print(f"\ti2c_port: {vtx_config_json['i2c_port']} -> 1")
                vtx_config_json["i2c_port"] = 1
                print(f"\trc_toggle: {vtx_config_json['rc_toggle']} -> 5")
                vtx_config_json["rc_toggle"] = 5
                continue
            elif field == "d0008_v5":
                print(f"\ti2c_port: {vtx_config_json['i2c_port']} -> 1")
                vtx_config_json["i2c_port"] = 1
                print(f"\trc_toggle: {vtx_config_json['rc_toggle']} -> 11")
                vtx_config_json["rc_toggle"] = 11
                continue
            elif field == "d0013":
                print(f"\ti2c_port: {vtx_config_json['i2c_port']} -> 0")
                vtx_config_json["i2c_port"] = 0 # Stinger V2 Supports I2C power and temperature monitoring on i2c-0: https://docs.modalai.com/M0195/#modalai-vtx-i2c-interface
                print(f"\trc_toggle: {vtx_config_json['rc_toggle']} -> 11")
                vtx_config_json["rc_toggle"] = 11
                continue
            elif field == "d0020_strike":
                print(f"\ti2c_port: {vtx_config_json['i2c_port']} -> 1")
                vtx_config_json["i2c_port"] = 1
                continue

            # MCS, Power, and Source based on vtx profile so handle that here
            if field == "power" or field  == "mcs" or field == "source":
                print(f"\t{field}: {vtx_config_json['profiles'][active_profile_idx][field]} -> {value}")
                vtx_config_json["profiles"][active_profile_idx][field] = value
            else:
                print(f"\t{field}: {vtx_config_json[field]} -> {value}")
                vtx_config_json[field] = value

        # Save changes to file
        save_json_with_header(VTX_CONFIG_FILE, vtx_config_json)
    except FileNotFoundError as e:
        print(f"[ERROR] Unable to locate VTX configuration file.")
        print(str(e))
        rc = -1
    except Exception as e:
        print(f"[ERROR] An unexpected error occurred while uploading VTX configuration. {str(e)}")
        if vtx_config_json is not None:
            save_json_with_header(VTX_CONFIG_FILE, vtx_config_json)
        rc = -1
    
    # Restart service with new settings if one of the presets 
    # if args.get("d0008_v4") or args.get("d0008_v5") or args.get("d0013"): restart_service(VTX_SERVICE_NAME)
    return rc

# Handle updating wifibroadcast configuration file via wizard
def wizard_update_vtx_config() -> bool:
    """
    Update parameters in VTX configuration file via wizard
    """
    options = []
    cases = {}
    invalid = False

    while True:
        try:
            read_file(VTX_CONFIG_FILE)
            vtx_config_json = load_json_with_comments(VTX_CONFIG_FILE)
            options = list(vtx_config_json.keys())
            options.insert(0,"Quit")
            options.insert(1,"Back")
            options.insert(2,"Restart Service")

            print(f"{LIT_GRN}Select a field to update:{RESET}")
            for index, option in enumerate(options):
                print(f"[{index + 1}] {option}")
                cases[index + 1] = option
    
            try:
                field = cases.get(int(input()))
                print(f"{CLEAR}", end="")
            except KeyboardInterrupt:
                print()
                exit(0)
            except:
                print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")
                continue
            
            if field == "Back": break
            if field == "Quit": exit(0)
            if field == "Restart Service": 
                restart_service(VTX_SERVICE_NAME)
                continue
            if field == "profiles":
                update_vtx_profile()
                continue
            with open(VTX_CONFIG_FILE, 'w') as vtx_config:
                try:
                    if type(vtx_config_json[field]) == bool:
                        value = input(f"Enter a value for {field} (Current: bool, {vtx_config_json[field]}): ").lower()
                        if value == "false":  value = False
                        elif value == "true": value = True
                        else:
                            throw_exception = 1/0
                    elif type(vtx_config_json[field]) == int:
                        value = int(input(f"Enter a value for {field} (Current: int, {vtx_config_json[field]}): "))
                    elif type(vtx_config_json[field]) == str:
                        value = input(f"Enter a value for {field} (Current: string, {vtx_config_json[field]}): ")
                    else:
                        print(f"Unknown type: {type(vtx_config_json[field])}")
                except KeyboardInterrupt:
                    print()
                    invalid = True
                    continue
                except:
                    print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")
                    invalid = True
                    continue

            if field == "frequency":
                if freq_to_ch.get(value, -1) == -1:
                    print(f"Invalid frequency provided: {value}.")
                    continue
                set_override_frequency(value)
                _ = subprocess.run(['voxl-send-command', 'vtx_latency', 'set_channel', shlex.quote(str(freq_to_ch.get(value)))], stdout=subprocess.DEVNULL)

            print(f"{field}: {vtx_config_json[field]} -> {value}")
            vtx_config_json[field] = value
            save_json_with_header(VTX_CONFIG_FILE, vtx_config_json)
        except FileNotFoundError as e:
            print(f"{RED}{BOLD}[ERROR] Unable to locate configuration file, please run voxl-vtx at least once so that it generates a configuration file.{RESET}")
            print(str(e))
            return False
        except Exception as e:
            if vtx_config_json is not None:
                save_json_with_header(VTX_CONFIG_FILE, vtx_config_json)
            if not invalid: 
                print(f"{RED}{BOLD}[ERROR] An unexpected error occurred while uploading configuration. {str(e)}{RESET}")
                return False
            invalid = False
    return True

# Wizard for handling interactive mode
def wizard() -> int:
    """
    Handle interactive mode, guide user through option menus and handle input.
    Wizard options:
    [1] Quit wizard
    [2] Restart VTX     -> Restart voxl-vtx service after making changes
    [3] VTX settings    -> Show current VTX settings
    """

    # Add additional Menu options here
    options = ["Quit", "Restart VTX", "VTX Settings"]
    cases = {}
    while True:
        print(f"{LIT_GRN}Select an option:{RESET}")
        for index, option in enumerate(options):
            print(f"[{index + 1}] {option}")
            cases[index + 1] = option

        try:
            ans = cases.get(int(input()))
            print(f"{CLEAR}", end="")
        except KeyboardInterrupt:
            print()
            exit(0)
        except:
            print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")
            continue
            
        # Handle request
        if   ans == "Quit":         return 0
        elif ans == "Restart VTX":  restart_service(VTX_SERVICE_NAME)
        elif ans == "VTX Settings": wizard_update_vtx_config()
        else:
            print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")
            continue

def should_update_vtx_config(args: dict) -> bool:
    return  args["frequency"] or args ["source"] or args["power"] or args["power_mw"] or args["d0008_v4"] or args["d0008_v5"] or args["d0013"] or args["d0020_strike"] or args["mcs"] != None and args["mcs"] >= 0 or args["active_profile_idx"] != None and args["active_profile_idx"] >= 0

if __name__ == "__main__":
    rc: int = 0
    args = parse_args()
    
    # Print help message if no args provided
    if all(value is None or value is False for value in args.values()) or args["help"]:	
        print_usage()
        exit(0)

    if args["disable"]: 
        disable_vtx()
        exit(0)

    if not os.path.exists(VTX_CONFIG_FILE):
        print(f"{WARN}[WARNING] Unable to locate VTX configuration file! Generating default configuration file now.{RESET}")
        write_factory_default_config()

    if os.path.exists(VTX_CONFIG_FILE) and os.path.getsize(VTX_CONFIG_FILE)<5 and not args["factory_enable"]: 
        print(f"{WARN}[WARNING] VTX configuration file may be empty (file size: {os.path.getsize(VTX_CONFIG_FILE)})! Generating default configuration file now.{RESET}")
        write_factory_default_config()

    # Dump current configuration
    if args["list"]: dump_config()
    
    # Check for use wizard (no args provided)
    if args["wizard"]: exit(wizard())
    
    # Handle swapping between wifi and softap (they all use the same network interface and will fight each other for control of it if more than one is running)
    if args["toggle"]: toggle_vtx_softap()
    
    # Handle swapping kernel modules
    if args["factory_enable"]: rc = factory_enable_vtx(args, True)
    if args["factory_enable_lte"]: rc = factory_enable_lte(args)
    if args["factory_enable_doodle"]: rc = factory_enable_doodle(args)
    if args["factory_enable_dtc"]: rc = factory_enable_dtc(args)
    if args["factory_enable_fiber"]: rc = factory_enable_fiber(args)
    if args["enable"]:  rc = enable_vtx()
    
    # Handle Wifibroadcast configuration 
    if should_update_vtx_config(args): rc = update_vtx_config(args)
    exit(rc)
