#!/usr/bin/python3

import argparse
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-DDP2Mast-MagCurrentComp.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_3x2_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: ' + DEFAULT_VERSION, required=False, type=str, default=DEFAULT_VERSION, 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

# 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, DEFAULT_VERSION]

	# 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"]
	
	# Print help message if no args provided
	if list(args.values()) == EMPTY_ARGS or args["help"]:	
		print_usage()
		exit(0)

	# 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()
