#!/usr/bin/python3

import argparse
import sys
import subprocess
import time
import os
import re
from typing import Tuple

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

PARAM_INT=6
PARAM_FLOAT=9

DEFAULT_VERSION = "v1.14"
VALID_VERSIONS = ["v1.10", "v1.11", "v1.12", "v1.13", "v1.14", "v1.18"]
PARAMS_BASE = "/usr/share/modalai/px4_params"

BASE_DIR = PARAMS_BASE + "/" + DEFAULT_VERSION
PLATFORM_DIR = BASE_DIR + "/platforms"
PARAMS_PATH = "/data/px4/param/parameters"
SKU = "/data/modalai/sku.txt"
PLATFORM_DIC = {
	"MRB-D0005-V2":("platforms/VOXL_2_defaults.params", \
					"platforms/D0005_Starling.params", \
					"EKF2_helpers/indoor_vio.params", \
					"battery_helpers/Sony_vtc6_2s.params",\
					"radio_helpers/Commando_8.params"),

	"MRB-D0006"   :("platforms/VOXL_2_defaults.params", \
					"platforms/Sentinel_V1.params",\
					"EKF2_helpers/outdoor_gps_baro.params"),

	"MRB-D0006-V2":("platforms/VOXL_2_defaults.params", \
					"platforms/Sentinel_V1.params", \
					"EKF2_helpers/outdoor_gps_baro.params", \
					"radio_helpers/Commando_8.params"),

	"MRB-D0008-V4":("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0008 V4 V5.params"),


	"MRB-D0008"   :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0008 V4 V5.params"),

	"MRB-D0008-V6"   :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0008_V6.params", \
					"battery_helpers/Molicel_P50b_6S.params", \
					"fpv_helpers/ekf2_universal_tweaks.params", \
					"fpv_helpers/ekf2_outdoor_stm_gps_mag.params", \
					"fpv_helpers/D0008_velocities.params", \
					"fpv_helpers/failsafes.params", \
					"fpv_helpers/disable_rtl.params", \
					"fpv_helpers/D0008_V6_short_mast_flat_battery_mag_compensation.params", \
					"fpv_helpers/rc_loadout_alt_vfc.params", \
					"fpv_helpers/enable_turtle_mode.params"),

	"MRB-D0008-V6-XS"   :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0008_V6.params", \
					"battery_helpers/Molicel_P50b_6S.params", \
					"fpv_helpers/ekf2_universal_tweaks.params", \
					"fpv_helpers/ekf2_outdoor_stm_gps_mag.params", \
					"fpv_helpers/D0008_velocities.params", \
					"fpv_helpers/failsafes.params", \
					"fpv_helpers/disable_rtl.params", \
					"fpv_helpers/D0008_V6_short_mast_flat_battery_mag_compensation.params", \
					"fpv_helpers/rc_loadout_manual_alt_position.params", \
					"fpv_helpers/enable_turtle_mode.params", \
					"fpv_helpers/D0008 V6 XS Payload Tweaks.params"),


	"MTB-D0005-V2":("platforms/VOXL_2_defaults.params", \
					"ci_helpers/ci_starling_v2.params"),

	"MTB-D0006"   :("platforms/VOXL_2_defaults.params", \
					"ci_helpers/ci_sentinel.params"),

	"MTB-D0008"   :("platforms/VOXL_2_defaults.params", \
					"ci_helpers/ci_fpv_revB.params"),

	"MRB-D0010"   :("platforms/VOXL_2_defaults.params", \
					"platforms/D0005_Starling.params", \
					"EKF2_helpers/indoor_vio_missing_gps.params", \
					"battery_helpers/Sony_vtc6_2s.params", \
					"radio_helpers/Commando_8.params"),

	"MRB-D0011"   :("platforms/VOXL_2_defaults.params", \
					"platforms/D0011_PX4_Autonomy_dev_kit.params", \
					"EKF2_helpers/indoor_vio.params", \
					"EKF2_helpers/exposed_baro.params", \
					"battery_helpers/Sony_vtc6_2s.params", \
					"radio_helpers/Commando_8.params"),

	"MRB-D0012"   :("platforms/VOXL_2_defaults.params", \
					"EKF2_helpers/vio_gps_baro.params", \
					"platforms/D0012_Starling_2_Max.params", \
					"EKF2_helpers/exposed_baro.params", \
					"battery_helpers/Amprius_18650_4s.params", \
					"other_helpers/starling_2_max_outdoor_position.params", \
					"radio_helpers/Commando_8.params"),

	"MRB-D0013"    :("platforms/VOXL_2_defaults.params", \
					"platforms/D0013.params", \
					"EKF2_helpers/ekf2_universal_tweaks.params", \
					"EKF2_helpers/exposed_baro.params", \
					"EKF2_helpers/no_vio_or_gps.params", \
					"battery_helpers/Molicel_P30b_4s.params", \
					"other_helpers/msp_dp_osd_skyvision.params", \
					"radio_helpers/tbs_tango2_stinger.params"),

	"MRB-D0013-V3" :("platforms/VOXL_2_defaults.params", \
					"platforms/D0013-V3.params", \
					"EKF2_helpers/ekf2_universal_tweaks.params", \
					"EKF2_helpers/exposed_baro.params", \
					"EKF2_helpers/no_vio_or_gps.params", \
					"battery_helpers/Molicel_P30b_4s.params", \
					"other_helpers/msp_dp_osd_skyvision.params", \
					"radio_helpers/tbs_tango2_stinger.params"),

	"MRB-D0014"   :("platforms/VOXL_2_defaults.params", \
					"EKF2_helpers/vio_gps_baro.params", \
					"platforms/D0014_Starling_2.params", \
					"EKF2_helpers/exposed_baro.params", \
					"battery_helpers/Amprius_18650_2s.params", \
					"other_helpers/starling_2_indoor_position.params", \
					"radio_helpers/Commando_8.params"),
					
	"MRB-D0015"   :("platforms/VOXL_2_defaults.params", \
					"platforms/D0015_Fixed_Wing.params"),

	"MRB-D0016"   :("platforms/VOXL_2_defaults.params", \
					"platforms/D0016_Sparrow.params", \
					"EKF2_helpers/indoor_vio_missing_gps.params", \
					"EKF2_helpers/shielded_baro.params", \
					"battery_helpers/Amprius_18650_2s.params", \
					"other_helpers/starling_2_indoor_position.params", \
					"radio_helpers/Commando_8.params"),

	"MRB-D0019"   :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0019.params", \
					"battery_helpers/Molicel_P50b_6S.params", \
					"fpv_helpers/ekf2_universal_tweaks.params", \
					"fpv_helpers/ekf2_outdoor_gps_baro_nomag.params", \
					"fpv_helpers/D0020_velocities.params", \
					"fpv_helpers/failsafes.params", \
					"fpv_helpers/rc_loadout_alt_position_offboard.params"),

	"MRB-D0020"   :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0020_V5.params", \
					"battery_helpers/Molicel_P50b_6S2P.params", \
					"fpv_helpers/ekf2_universal_tweaks.params", \
					"fpv_helpers/ekf2_outdoor_gps_baro_nomag.params", \
					"fpv_helpers/D0020_velocities.params", \
					"fpv_helpers/failsafes.params", \
					"fpv_helpers/enable_rtl.params", \
					"fpv_helpers/rc_loadout_alt_position_offboard.params"),
	
	"MRB-D0020-XS" :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0020_V5.params", \
					"battery_helpers/Molicel_P50b_6S2P.params", \
					"fpv_helpers/ekf2_universal_tweaks.params", \
					"fpv_helpers/ekf2_outdoor_gps_baro_nomag.params", \
					"fpv_helpers/D0020_velocities.params", \
					"fpv_helpers/failsafes.params", \
					"fpv_helpers/rc_loadout_alt_position_position.params",\
					"fpv_helpers/disable_turtle_mode.params"),

	"MRB-D0020-V6"  :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0020_V6.params", \
					"battery_helpers/Molicel_P50b_6S2P.params", \
					"fpv_helpers/ekf2_universal_tweaks.params", \
					"fpv_helpers/ekf2_outdoor_stm_gps_mag.params", \
					"fpv_helpers/D0020_velocities.params", \
					"fpv_helpers/failsafes.params", \
					"fpv_helpers/enable_rtl.params", \
					"fpv_helpers/rc_loadout_alt_position_offboard.params", \
					"fpv_helpers/disable_turtle_mode.params"),

	"MRB-D0020-V6-XS" :("platforms/VOXL_2_defaults.params", \
					"platforms/VOXL_2_fpv_defaults.params", \
					"platforms/D0020_V6.params", \
					"battery_helpers/Molicel_P50b_6S2P.params", \
					"fpv_helpers/ekf2_universal_tweaks.params", \
					"fpv_helpers/ekf2_outdoor_stm_gps_mag.params", \
					"fpv_helpers/D0020_velocities.params", \
					"fpv_helpers/failsafes.params", \
					"fpv_helpers/rc_loadout_manual_alt_alt.params"),

	"HITL"        :("other_helpers/hitl.params", \
					"platforms/HITL_Iris.params"),
}

CAL_PARAMS_PATH = "/data/px4/param"
CAL_PARAM_FILES = [ "parameters_gyro.cal", "parameters_acc.cal", "parameters_level.cal", "parameters_mag.cal", "parameters_rc.cal", "parameters_baro.cal", "parameters_baro_tc.cal", "parameters_imu_tc.cal" ]

# 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('-n', '--non-interactive', help='Non-interactive mode for scripting', required=False, action='store_false')
	parser.add_argument('-w', '--wizard', help='Activate interactive wizard', required=False, action='store_true')
	parser.add_argument('-q', '--quiet', help="Quiet mode, don't print debug msgs", required=False, action='store_true')
	parser.add_argument('-f', '--file', help='Px4-parameters file path (.params file)', required=False, type=str)
	parser.add_argument('-p', '--platform', help='Product platform to set params for', required=False, type=str)
	parser.add_argument('-s', '--skip-cal', help='Skip loading calibration parameters', required=False, action='store_true')
	parser.add_argument('-v', '--version', help='PX4 parameter version (e.g. v1.14, v1.18). Default: auto-detect installed PX4, fall back to ' + DEFAULT_VERSION, required=False, type=str, default=None, choices=VALID_VERSIONS)
	args = vars(parser.parse_args())
	
	if args["platform"] and args["platform"] not in PLATFORM_DIC:
		print(f"{RED}[ERROR] Platform {args['platform']} not supported. Please enter one of the following:{RESET}")
		print(', '.join(PLATFORM_DIC.keys()))
		exit(1)

	if not args["non_interactive"] and not args["platform"] and not args["file"]:
		print(f"{RED}[ERROR] Platform type or file path must be specified when using non-interactive mode. {RESET}")
		exit(1)

	return args

# Detect the installed PX4 version from the px4 binary itself
def detect_px4_version() -> str:
	"""
	Determine which params version dir matches the PX4 installed on this drone.

	Primary method: read the version tag embedded in /usr/bin/px4 (the real
	ELF; /usr/bin/voxl-px4 is a wrapper script). This reflects the binary that
	actually runs, regardless of what the dpkg database claims - multiple px4
	packages can coexist in dpkg (one Replaces another without purging its
	entry), leaving stale file ownership or silently skipped installs, so dpkg
	can disagree with the bytes on disk in both directions.

	Fallback: dpkg ownership of /usr/bin/voxl-px4 (correct whenever the
	database is clean, i.e. only one px4 package installed).

	@return: a VALID_VERSIONS entry (e.g. "v1.18"), otherwise DEFAULT_VERSION.
	"""
	# Primary: version tag embedded in the binary (e.g. v1.18.0-beta2,
	# v1.14.0-2.0.142-dev-dirty). Trust it only if all tags in the binary
	# agree on a single major.minor.
	try:
		with open("/usr/bin/px4", "rb") as f:
			tags = set(re.findall(rb"v(1\.\d+)\.\d+", f.read()))
		if len(tags) == 1:
			ver = "v" + tags.pop().decode()
			if ver in VALID_VERSIONS:
				return ver
	except Exception:
		pass
	# Fallback: dpkg file ownership
	try:
		pkg = subprocess.run(["dpkg", "-S", "/usr/bin/voxl-px4"],
			capture_output=True, text=True, timeout=5).stdout.split(":")[0].strip()
		if not pkg:
			pkg = "voxl-px4"
		ver = subprocess.run(["dpkg-query", "-W", "-f=${Version}", pkg],
			capture_output=True, text=True, timeout=5).stdout.strip()
		if ver.startswith("1.18"):
			return "v1.18"
	except Exception:
		pass
	return DEFAULT_VERSION

# Print help message
def print_usage() -> None:
	"""
	Print usage/help message
	"""
	print("Description: Parse a px4-parameters file and set the params listed in the file.")
	print("")
	print("Usage: voxl-configure-px4-params -h -n -w -q -f [file path] -p [platform] -v [version]")
	print("")
	print("Optional Arguments:")
	print(" -h, --help              Show this help message and exit")
	print(" -n, --non-interactive   Non-interactive mode for scripting")
	print(" -w, --wizard            Activate interactive wizard")
	print(" -q, --quiet             Quiet mode, don't print msgs")
	print(" -f, --file              File path to a .params file")
	print(" -p, --platform          do full config for a specfic platform")
	print(" -s, --skip-cal          Skip loading calibration parameters")
	print(" -v, --version           PX4 parameter version (default: " + DEFAULT_VERSION + ")")
	print("                         Valid versions: " + ", ".join(VALID_VERSIONS))
	print("")
	print("")
	print(" Valid strings for full platform config:")
	print("   MRB-D0005-V2 (Starling V2)")
	print("   MRB-D0006    (Sentinel V1)")
	print("   MRB-D0008    (FPV RevB)")
	print("   MRB-D0011    (Starling PX4 Edition)")
	print("   MRB-D0013")
	print("")
	print("")

# Print message 
def _print(msg: str) -> None:
	"""
	Print message and handle quiet mode

	@msg: Message to be printed.
	@quiet: Don't print msgs if True 
	"""
	if not QUIET:
		print(msg)

# Check if a process is running
def is_process_running(proc_name: str) -> bool:
    """
    Checks if a process with the given name is currently running.

    @proc_name: name of process to check
    @return: True/False depending on process state
    """
    # -x ensures exact match for the process name
    result = subprocess.run(['pgrep', '-x', proc_name], stdout=subprocess.DEVNULL, check=False)
    return result.returncode == 0

# Read a line from a file
def read_file_line(filename: str) -> str:
    """
    reads a line from a file

    @filename: path of file to read
    @return: file content
    """ 
    if not os.path.exists(filename):
        return None
    
    with open(filename) as f:
        content = f.read().strip()
        return content

# Restart voxl-px4 when it fails while uploading PX4 parameters
def restart_voxl_px4() -> None:
	"""
	Restart voxl-px4
	"""
	# Yes, you could just run systemctl restart voxl-px4, but I have found that to be unreliable
	stop_command = "systemctl stop voxl-px4"
	start_command = "systemctl start voxl-px4"

	subprocess.run(stop_command.split(), stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
	time.sleep(5)
	subprocess.run(start_command.split(), stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
	time.sleep(10)

# Trim .cal file of duplicate entries  
def trim(calibration_file: str):
	"""
	Trim a calibration file of duplicate entries in order to avoid overwriting intended calibration params

	@calibration_file: File of calibration parameters. **SHOULD ALREADY BE AN OPENED FILE!**
	"""
	cal_param_list = []
	trim_lines : int = []

	parameters = calibration_file.readlines()
	parameters.reverse()
	total_lines = len(parameters) -1

	# Mark duplicate lines that need to be removed 
	for current_line, parameter in enumerate(parameters):
		if "#" in parameter.strip("\n") or parameter == "\n":
			continue
		
		cal_param = parameter.strip("\n").split()[2]
		cal_value = parameter.strip("\n").split()[3]
		
		# Mark duplicate for removal
		if cal_param in cal_param_list:
			trim_lines.append(total_lines - current_line)

		# Mark sensors w/ ID of 0 for removal 
		elif "_ID" in cal_param and int(cal_value) == 0:
			trim_lines.append(total_lines - current_line)
		
		# Otherwise, add to list of params we have already seen 
		else:
			cal_param_list.append(cal_param)
	
	# Just return file in read mode if no trimming needed
	if not trim_lines:
		return calibration_file

	# Close the file and reopen in write mode if trimming needed
	calibration_file.close()
	trim_lines.reverse()

	# Write back only the lines we didn't mark to be trimmed in previous section
	with open(calibration_file.name, 'w') as trimmed_file:
		_print(f"[INFO] Trimming duplicate parameters...")
		parameters.reverse()
		for index, line in enumerate(parameters):
			if index not in trim_lines:
				trimmed_file.write(line)

	# Reopen and return file in read mode
	return open(calibration_file.name)

# Old-style IMU thermal cal parameter names mapped to their current PX4 equivalents.
# The linear slope params (S*) map to the first-order polynomial coefficients (X1_*).
# See qrb5165-imu-server commit 7b906afa for the naming fix this mirrors.
IMU_TC_OLD_TO_NEW = {
	"IMU_TC_A_EN":   "TC_A_ENABLE",
	"IMU_TC_G_EN":   "TC_G_ENABLE",
	"IMU_TC_A_TREF": "TC_A0_TREF",
	"IMU_TC_G_TREF": "TC_G0_TREF",
	"IMU_TC_A_SX":   "TC_A0_X1_0",
	"IMU_TC_A_SY":   "TC_A0_X1_1",
	"IMU_TC_A_SZ":   "TC_A0_X1_2",
	"IMU_TC_G_SX":   "TC_G0_X1_0",
	"IMU_TC_G_SY":   "TC_G0_X1_1",
	"IMU_TC_G_SZ":   "TC_G0_X1_2",
}

# The old cal procedure did not record a valid temperature range; PX4 clamps
# the compensation polynomial to [TMIN, TMAX] so it must cover operating temps.
IMU_TC_TMIN = 0.0
IMU_TC_TMAX = 80.0

# Read a .cal file into a dict
def read_cal_params(cal_file: str) -> dict:
	"""
	Parse a PX4 .cal file (<group> <instance> <name> <value> <type> per line)
	into a {name: value} dict. Values are kept as strings.

	@cal_file: path of the calibration file
	@return: dict of parameter name to value, empty if the file is missing
	"""
	params = {}
	if not os.path.exists(cal_file):
		return params

	with open(cal_file) as f:
		for line in f:
			fields = line.strip().split()
			if len(fields) >= 4 and not line.startswith("#"):
				params[fields[2]] = fields[3]

	return params

# Migrate an old-format IMU thermal cal file to current PX4 parameter names
def migrate_imu_tc_cal(tc_cal_file: str = "/data/px4/param/parameters_imu_tc.cal",
			acc_cal_file: str = "/data/px4/param/parameters_acc.cal",
			gyro_cal_file: str = "/data/px4/param/parameters_gyro.cal") -> bool:
	"""
	Migrate an IMU thermal cal file written with pre-fix parameter names
	(IMU_TC_*) to the current PX4 names (TC_A_ENABLE / TC_A0_* / TC_G0_*).

	The measured values (reference temperature and per-axis linear slopes)
	carry over unchanged. The new format additionally requires the accel and
	gyro device IDs, without which PX4 will not apply the compensation; these
	are taken from the accel/gyro cal files that sit next to the thermal cal.
	Unmeasured polynomial coefficients (X0, X2, X3) are written as zero.
	The original file is preserved as <tc_cal_file>.bak.

	@tc_cal_file: thermal cal file to migrate in place
	@acc_cal_file: accel cal file providing CAL_ACC0_ID
	@gyro_cal_file: gyro cal file providing CAL_GYRO0_ID
	@return: True if the file was migrated, False otherwise (already current,
		missing inputs, or nothing to do)
	"""
	old = read_cal_params(tc_cal_file)

	if not old:
		_print(f"[INFO] No thermal cal file at {tc_cal_file}, nothing to migrate")
		return False

	if not any(name in old for name in IMU_TC_OLD_TO_NEW):
		_print(f"[INFO] {tc_cal_file} already uses current TC_* parameter names")
		return False

	missing = [name for name in IMU_TC_OLD_TO_NEW if name not in old]
	if missing:
		_print(f"[ERROR] {tc_cal_file} is missing expected parameters: {', '.join(missing)}")
		return False

	acc_id = read_cal_params(acc_cal_file).get("CAL_ACC0_ID")
	gyro_id = read_cal_params(gyro_cal_file).get("CAL_GYRO0_ID")

	if acc_id is None or gyro_id is None:
		_print(f"[ERROR] Could not read CAL_ACC0_ID from {acc_cal_file} and/or CAL_GYRO0_ID from {gyro_cal_file}")
		return False

	def sensor_entries(prefix: str, enable_name: str, enable_val: str, sensor_id: str, tref: str, slopes: list) -> list:
		entries = [
			(enable_name, enable_val, PARAM_INT),
			(f"{prefix}_ID", sensor_id, PARAM_INT),
			(f"{prefix}_TREF", tref, PARAM_FLOAT),
			(f"{prefix}_TMIN", f"{IMU_TC_TMIN:.6f}", PARAM_FLOAT),
			(f"{prefix}_TMAX", f"{IMU_TC_TMAX:.6f}", PARAM_FLOAT),
		]

		for order in range(4):
			for axis in range(3):
				value = slopes[axis] if order == 1 else "0.000000"
				entries.append((f"{prefix}_X{order}_{axis}", value, PARAM_FLOAT))

		return entries

	entries = sensor_entries("TC_A0", "TC_A_ENABLE", old["IMU_TC_A_EN"], acc_id, old["IMU_TC_A_TREF"],
				[old["IMU_TC_A_SX"], old["IMU_TC_A_SY"], old["IMU_TC_A_SZ"]])
	entries += sensor_entries("TC_G0", "TC_G_ENABLE", old["IMU_TC_G_EN"], gyro_id, old["IMU_TC_G_TREF"],
				[old["IMU_TC_G_SX"], old["IMU_TC_G_SY"], old["IMU_TC_G_SZ"]])

	os.replace(tc_cal_file, tc_cal_file + ".bak")

	with open(tc_cal_file, "w") as f:
		for name, value, param_type in entries:
			f.write(f"1\t1\t{name}\t{value}\t{param_type}\n")

	_print(f"[INFO] Migrated {tc_cal_file} to current TC_* parameter names (backup at {tc_cal_file}.bak)")
	return True

# Validate voxl-px4 service state
def validate_voxl_px4() -> int:
	"""
	Validate that the voxl-px4 service is running on target.
	"""
	
	start_voxl_px4_command = ['systemctl', 'start', 'voxl-px4']

	if is_process_running("voxl-px4"):
		_print(f"[INFO] Voxl-px4 process found and active.")
	else:
		_print(f"[INFO] Voxl-px4 not currently running.")

		# Prompt user for input until valid response is given
		while True:
			if INTERACTIVE:
				print(f"\nWould you like to start voxl-px4 now? (y/n)")
				ans = input()
				if ans.lower() == "y":
					_print(f"\n[INFO] Starting voxl-px4 service...")
					subprocess.run(start_voxl_px4_command, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
					time.sleep(12)
					if is_process_running("voxl-px4"):
						_print(f"[INFO] Voxl-px4 started successfully!")
						break
					else:
						_print(f"{RED}[ERROR] Voxl-px4 failed to start successfully.{RESET}")
						exit(3)

				elif ans.lower() == "n":
					_print(f"{RED}[ERROR] Voxl-px4 must be running to update params. Exiting now.{RESET}")
					exit(4)

				else:
					print(f"{WARN}Invalid answer provided. Please enter y or n.{RESET}")
			else:
				_print(f"[INFO] Starting voxl-px4 service...")
				subprocess.run(start_voxl_px4_command, stdout=subprocess.PIPE)
				time.sleep(8)

				if is_process_running("voxl-px4"):
					_print(f"[INFO] Voxl-px4 started successfully!")
					break
				else:
					_print(f"{RED}[ERROR] Voxl-px4 failed to start successfully.{RESET}")
					exit(3)
	return 0

# Validate px4 params
def validate_params(params_file) -> bool:
	"""
	Validate that recently loaded parameters were uploaded successfully.

	@params_file: File of parameters to compare with currently loaded parameters
	@return: True/False, whether parameters were uploaded successfully 
	"""

	# Return to start of file 
	params_file.seek(0)

	# Compare params currently on target with those in provided file
	scan_params_command = ['px4-param', 'show', "-a"]

	ret=True;

	# Get currently loaded parameters
	try:
		scan = subprocess.check_output(scan_params_command).decode('utf-8')
	except:
		return False
		
	# Compare currently loaded parameters with ones that were just uploaded
	for line in params_file:
		if "#" not in line and "/" not in line and line != "\n":
			vars = line.strip("\n").split()
			param = vars[-3]
			val = vars[-2]
			param_type = vars[-1]

			if param in scan:
				match = re.search(rf"{param}\s+\[[^]]*\]\s+:\s*(-?\d+(?:\.\d+)?)", scan)

				if not match:
					_print(f"{param} doesn't match 1")
					ret = False
					continue

				size = len(val) if len(match.group(1)) < len(val) else len(match.group(1))

				# Format both values the same
				if param_type == PARAM_INT:
					val = round(int(val[:size]),4)
					cur = round(int(match.group(1)[:size]),4)
				else:
					val = round(float(val[:size]),4)
					cur = round(float(match.group(1)[:size]),4)

				# If params don't match then the upload failed
				if cur != val:
					_print(f"{param} doesn't match 2 {cur} {val}")
					ret = False
					continue
				
			# Parameter doesn't exist
			else:
				# _print(f"{param} not in scan")
				# return False
				continue

	return ret

# Generate dictionary of px4 params that will be updated
def filter_params(params_file) -> Tuple[bool, dict]:
	"""
	Determine which parameters in the provided params file have different values
	than the value already loaded. 
	
	@params_file: File of parameters to be loaded.
	@return:
		True/False to continue with uploading listed param differences\n
		Dictionary of parameters that are different and their values
	"""

	# Compare params currently on target with those in provided file
	scan_params_command = ['px4-param', 'show', '-a']
	diff = {}
	current = []
	_print(f"[INFO] Scanning currently loaded parameters...")
	
	try:
		scan = subprocess.check_output(scan_params_command).decode('utf-8')
	except:
		return False, None
		
	for line in params_file:
		if "#" not in line and "/" not in line and line != "\n":
			vars = line.strip("\n").split()
			param = vars[-3]
			val = vars[-2]
			param_type = vars[-1]

			if param in scan:
				match = re.search(rf"{param}\s+\[[^]]*\]\s+:\s*(-?\d+(?:\.\d+)?)", scan)

				if not match:
					continue
			
				size = len(val) if len(match.group(1)) < len(val) else len(match.group(1))

				# Not matching param
				if param_type == PARAM_INT:
					val = round(int(val[:size]),4)
					cur = round(int(match.group(1)[:size]),4)
				else:
					val = round(float(val[:size]),4)
					cur = round(float(match.group(1)[:size]),4)

				if cur != val:
					diff[param] = str(val) 
					current.append(str(cur))
			else:
				diff[param] = str(val) 
				current.append("N/A")

	if diff:
		# Show differences 	
		_print(f"{WARN}{BOLD}The following parameters about to be loaded differ from those currently loaded:{RESET}")
		_print(f"{WARN}{BOLD}\tNAME    \t\tCURRENT    \tNEW{RESET}")
		for index, key in enumerate(diff.keys()):
			key_spacing = "    \t\t" if len(key) < 12 else "    \t"
			val_spacing = "\t\t" if len(str(current[index])) < 8 else "\t"
			_print(f"{WARN}[{index + 1}]\t{key}{key_spacing}{current[index]}{val_spacing}{diff[key]}{RESET}")

		# If non-interactive mode then don't ask for verification
		if INTERACTIVE:
			while True:
				print("\nWould you like to continue with the parameter differences listed above? (y/n)")
				ans = input()
				if ans.lower() == "y":
					return True, diff
				elif ans.lower() == "n":
					return False, diff
				else:
					print(f"{WARN}Invalid answer provided, please enter y or n.{RESET}")
		else:
			return True, diff

	else:
		_print(f"[INFO] No difference in parameters was detected.")
		return False, diff

# Set params 
def set_params(params_file:str) -> int:
	"""
	Parse a tab separated params file containing px4-parameters. Set the parameters with their corresponding values and types in voxl-px4.
	
	@params_file: A tab separated .params file which contains the relevant data
	"""

	assert ".params" in params_file or ".cal" in params_file, f"{RED}[ERROR] Input file must be a .params or .cal file!{RESET}"

	if not os.path.exists(params_file):
		print(f"{RED}[ERROR] Params file {params_file} not found!{RESET}")
		return False, None

	_print(f"\n[INFO] Loading file: {params_file}")
	set_param_command = ['px4-param', 'set', '','']
	max_attempts = 5
	invalid_params = {}
	with open(params_file) as params:

		# Trim duplicate params
		if params.name.split('/')[-1] in CAL_PARAM_FILES:
			params.seek(0)
			params = trim(calibration_file=params)
			params.seek(0)

		# Filter params to be uploaded
		result, diff = filter_params(params)
		
		# Quit set params process if user doesn't want to continue after seeing diff list or no differences detected
		if not result:
			return 0

		_print(f"\n[INFO] Setting params...")

		# Case when params file was just wiped
		if diff is None:
			restart_voxl_px4()
			for line in params:
				if "#" not in line and line != "\n":
					vars = line.strip("\n").split()
					val = vars[-2]
					param = vars[-3]	
	
					set_param_command[-2] = param
					set_param_command[-1] = val

					# Try to upload PX4 Parameters, retry up to max_attempts times if failure
					for attempt in range(max_attempts):
						res = subprocess.run(set_param_command, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
						res_stdout = res.stdout.decode("utf-8")
						if not res.returncode:
							if "ERROR" in res_stdout and "not found" in res_stdout:
								invalid_params[param] = val
							break
						else: 
							restart_voxl_px4()

					# If we fail after max_attempts tries to upload PX4 Parameters then exit 
					if res.returncode:
						print(f"{RED}[ERROR] Failed to set PX4 parameters. Check voxl-px4 status for details.{RESET}")
						exit(3)

		# Upload parameters
		else:
			for key in diff.keys():
				set_param_command[-2] = key
				set_param_command[-1] = diff[key]

				# Try to upload PX4 Parameters, retry up to max_attempts times if failure
				for attempt in range(max_attempts):
					## run set twice, sometimes the first one fails
					subprocess.run(set_param_command, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
					res = subprocess.run(set_param_command, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
					res_stdout = res.stdout.decode("utf-8")
					if not res.returncode:
						if "ERROR" in res_stdout and "not found" in res_stdout:
							invalid_params[key] = diff[key]
						break
					else: 
						restart_voxl_px4()
				
				# If we fail after max_attempts tries to upload PX4 Parameters then exit 
				if res.returncode:
					print(f"{RED}[ERROR] Failed to set PX4 parameters. Check voxl-px4 status for details.{RESET}")
					exit(3)

			# Double check that all params in current file were uploaded correctly
			time.sleep(1) ## important to sleep, takes some time for params to take effect
			if validate_params(params):
				_print(f"{GRN}[INFO] PX4 parameter configuration successful!{RESET}")
			else:
				print(f"{RED}[ERROR] param validation failed. Check voxl-px4 status for details.{RESET}")
				print(f"{RED}[ERROR] likely SLPI is in a weird state, power cycle VOXL and try again.{RESET}")
				exit(-1)

			# Show any params that don't exist
			if invalid_params:
				_print(f"{WARN}{BOLD}[WARNING] The following parameters don't exist in your current version of PX4 and weren't loaded:{RESET}")
				_print(f"{WARN}{BOLD}\tNAME    \t\tVALUE{RESET}")
				for index, key in enumerate(invalid_params.keys()):
					key_spacing = "    \t\t" if len(key) < 12 else "    \t"
					_print(f"{WARN}[{index + 1}]\t{key}{key_spacing}{invalid_params[key]}{RESET}")
				_print("\n")

		# Trim duplicate params again since px4 will have just written duplicates back
		if params.name.split('/')[-1] in CAL_PARAM_FILES:
			params.seek(0)
			params = trim(calibration_file= params)

	return 0

# Show param upload options based on given directory
def show_options(directory: str) -> str:
	"""
	Show user possible options to select based on the current directory.

	@directory: The current directory in which we are looking.
	@return: The next directory to move to, or exit command.
	"""
	cases = {}
	options = os.listdir(directory)
	parent_directory = os.path.dirname(directory)
	
	# Allow user to quit from any menu
	options.insert(0,"Quit")

	# Provided extra options based on current directory
	if directory == BASE_DIR:
		options.insert(1,"Reset ALL PX4 params to default")
		options.append("calibration files")

	elif "platforms" in directory:
		options.insert(1,"Back")

	elif directory == CAL_PARAMS_PATH:
		options.insert(1,"Back")
		options.remove("parameters")
		options.append("All calibration files")

	else:
		options.insert(1,"Back")

	while True:
		if (directory == BASE_DIR) or (directory == CAL_PARAMS_PATH):
			option = "an option"
		else:
			option = "a " + directory.split('/')[-1].replace("_"," ")[:-1]

		# List options
		print(f"{LIT_GRN}Select {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}")
		except:
			print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")
			continue

		# Validate input
		if ans == None:
			print(f"{WARN}Invalid answer provided, please select a valid option.{RESET}")

		# Set params
		elif ".params" in ans:
			set_params(directory + "/" + ans)
			return parent_directory
		
		# Return to parent directory
		elif ans == "Back":
			if directory == CAL_PARAMS_PATH:
				return BASE_DIR
			
			return parent_directory 
		
		# Set some sort of reset or default params
		elif ans == "Reset ALL PX4 params to default":
			return "Reset"
		
		# Set some sort of reset or default params
		elif ans == "calibration files":
			return CAL_PARAMS_PATH
		
		# Quit
		elif ans == "Quit":
			return None
		
		# Navigate to new directory
		else:
			return directory + "/" + ans


def reset_no_question():

	## This method uses the param reset command
	## This does NOT work reliably, so instead wipe the whole file
	# print("Resetting params to default now....")
	# reset_param_command = ['px4-param', 'reset_all']
	# subprocess.run(reset_param_command, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)


	_print(f"[INFO] Wiping params file...")
	if os.path.exists(PARAMS_PATH):
		os.remove(PARAMS_PATH)
	_print(f"[INFO] Restarting voxl-px4 service...")
	restart_voxl_px4()
	time.sleep(5)

	print("done resetting params\n\n")


# Handle reset procedure
def reset() -> str:
	"""
	Handle the case when a user selects to reset params to default or wipe clean

	@return: The next directory to move to, or exit command.
	"""

	if INTERACTIVE:
		while True:
			print("\nWould you like to reset ALL px4 params to default? (y/n)")
			print("This includes calibration params, but those are backed up.")
			ans = input()
			if ans.lower() == "y":
				reset_no_question()
				break
			elif ans.lower() == "n":
				break
			else:
				print(f"{WARN}Invalid answer provided, please enter y or n.{RESET}")
	else:
		reset_no_question

	return BASE_DIR



# Wizard for handling interactive mode
def wizard() -> int:
	"""
	Handle interactive mode, guide user through option menus and handle input.
	"""
	directory = BASE_DIR
	while True:
		# Show options for current directory, receive new directory to move to 
		directory = show_options(directory)
		
		if directory == None:
			return 0
	
		if directory == "Reset":
			directory = reset()

		if ".cal" in directory:
			set_params(directory)
			directory = CAL_PARAMS_PATH

		if "All calibration files" in directory:
			for i in CAL_PARAM_FILES:
				curr_cal_file = CAL_PARAMS_PATH + "/" + i
			
				if os.path.isfile(curr_cal_file):
					set_params(curr_cal_file)

			directory = CAL_PARAMS_PATH

		# Quit process
		if directory is None:
			break
	
	return 0
		

if __name__ == "__main__":
	args = parse_args()
	QUIET = args["quiet"]
	EMPTY_ARGS = [False, True, False, False, None, None, False, None]

	# Print help message if no args provided
	if list(args.values()) == EMPTY_ARGS or args["help"]:	
		print_usage()
		exit(0)

	# Resolve params version: explicit -v wins, otherwise detect installed PX4
	if args["version"] is None:
		args["version"] = detect_px4_version()

	# v1.18 uses a new folder layout, handled by its own script
	if args["version"] == "v1.18":
		script_18 = os.path.join(os.path.dirname(os.path.abspath(__file__)), "voxl-configure-px4-params-18")
		if os.path.isfile(script_18):
			if not QUIET:
				print("[INFO] PX4 v1.18 detected, switching to voxl-configure-px4-params-18", flush=True)
			os.execv(sys.executable, [sys.executable, script_18] + sys.argv[1:])
		print(f"{RED}[ERROR] v1.18 params require voxl-configure-px4-params-18, which was not found.{RESET}")
		exit(1)

	# Set BASE_DIR based on chosen version
	BASE_DIR = PARAMS_BASE + "/" + args["version"]
	PLATFORM_DIR = BASE_DIR + "/platforms"
	
	# By default this arg is True, which means INTERACTIVE by default is True
	# The --non-interactive flag will set args["non-interactive"] to be False.
	INTERACTIVE = args["non_interactive"]

	# Check that voxl-px4 is running, prompt user for action if service is not running
	validate_voxl_px4()
	
	# Handle load params by specific file
	if args["file"]:
		set_params(args["file"])
		exit(0)
	
	# Handle load params by specific platform
	if args["platform"]:
		param_list=PLATFORM_DIC[args["platform"][:]]
		nl = '\n'
		print(f"{nl}This process will do the following:\n")
		print(f"1. Load the following param files: {nl}{nl.join(param_list)}\n")
		if not args["skip_cal"]:
			print(f"2. Restore calibration params\n")

		if INTERACTIVE:
			while True:
				print("\nWould you like to continue? (y/n)")
				ans = input()
				if ans.lower() == "y":
					break
				elif ans.lower() == "n":
					exit(0)
				else:
					print(f"{WARN}Invalid answer provided, please enter y or n.{RESET}")

		INTERACTIVE = False

		# DON"T reset params here anymore. we do this before voxl-px4 is configured
		# during the voxl-configure-mpa process as it's more reliable than resetting
		# px4 at this stage which would require a voxl-px4 restart to take effect
		# reset_no_question()

		# migrate an IMU thermal cal written by the old cal procedure (pre
		# name-fix) so the cal restore below loads current PX4 param names
		imu_tc_file = CAL_PARAMS_PATH + "/parameters_imu_tc.cal"
		if any(name in read_cal_params(imu_tc_file) for name in IMU_TC_OLD_TO_NEW):
			_print(f"[INFO] Old-style IMU thermal cal detected, migrating...")
			migrate_imu_tc_cal(imu_tc_file,
					CAL_PARAMS_PATH + "/parameters_acc.cal",
					CAL_PARAMS_PATH + "/parameters_gyro.cal")

		for param_file in param_list:
			set_params(BASE_DIR + "/" + param_file)

		if args["skip_cal"]:
			print(f"Skipping calibration parameters.")
		else:
			for cal_file in CAL_PARAM_FILES:
				curr_cal_file = CAL_PARAMS_PATH + "/" + cal_file

				if os.path.isfile(curr_cal_file):
					set_params(curr_cal_file)

		_print(f"{GRN}[INFO] Done configuring for platform " + args["platform"] + f"{RESET}")
		_print(f"{GRN}[INFO] make sure to fully power cycle VOXL before flying.{RESET}")
		exit(0)

	# Start interactive process
	if args["wizard"]:
		wizard()
