#!/usr/bin/python3

################################################################################
# Copyright 2025 ModalAI Inc.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
#    this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
#    this list of conditions and the following disclaimer in the documentation
#    and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its contributors
#    may be used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# 4. The Software is used solely in conjunction with devices provided by
#    ModalAI Inc.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
################################################################################

import os
from os import path
import json
import subprocess
import argparse
import time
import re

# 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[21m"

# This file is setup via voxl-configure-mpa
SKU = "/data/modalai/sku.txt"
VOXL_PX4_SERVICE_FILE = "/etc/systemd/system/voxl-px4.service"
VOXL_FLIR_SERVER_INTRINSICS_FILE = "/data/modalai/opencv_lepton0_raw_intrinsics.yml"

_debug = False

_px4_model = {
    "result" : False,
    "running" : False,
    "error" : False,
    "execStart" : "",
    "sensor_accel" : {
        "result" : False,
        "x": 0.0,
        "y": 0.0,
        "z": 0.0
    },
    "sensor_gyro" : {
        "result" : False,
        "x": 0.0,
        "y": 0.0,
        "z": 0.0
    },
    "sensor_baro" : {
        "result" : False,
        "temperature" : 0.0,
        "pressure" : 0.0,
    },
    "battery_status" : {
        "result" : False,
        "voltage_filtered_v" : 0.0
    }
}

_d0004_model = {
    "calOk" : False,
    "compute" : {
        "imageSensor0" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor1" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor2" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor3" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor4" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor5" : {
            "probe" : "error",
            "result" : False
        }
    },
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
        "stereo_front" : False,
        "stereo_rear" : False,
        "tracking" : False,
        "hires_large_color" : False,
        "hires_large_grey" : False,
        "hires_large_encoded" : False,
        "hires_small_color" : False,
        "hires_small_grey" : False,
        "hires_small_encoded" : False,
        "stereo_frontPipe" : "error",
        "stereo_rearPipe" : "error",
        "trackingPipe" : "error",
        "hires_large_colorPipe" : "error",
        "hires_large_greyPipe" : "error",
        "hires_large_encodedPipe" : "error",
        "hires_small_colorPipe" : "error",
        "hires_small_greyPipe" : "error",
        "hires_small_encodedPipe" : "error",
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-px4-imu-server" : {
        "result" : False,
        "mpaPipe" : "error",
        "running" : False
    },
    "voxl-qvio-server" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixed" : "error",
        "mpaPipeLocal" : "error",
        "mpaPipeFixedPoseInput" : "error"
    }
}

_d0005_model = { # Starling
    "calOk" : False,
    "compute" : {
        "imageSensor0" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor1" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor2" : {
            "probe" : "error",
            "result" : False
        }
    },
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
        "tof_conf" : False,
        "tof_pc" : False,
        "tof_depth" : False,
        "tof_ir" : False,
        "tracking" : False,
        "hires_large_color" : False,
        "hires_large_grey" : False,
        "hires_large_encoded" : False,
        "hires_small_color" : False,
        "hires_small_grey" : False,
        "hires_small_encoded" : False,
        "tof_confPipe" : "error",
        "tof_pcPipe" : "error",
        "tof_depthPipe" : "error",
        "tof_irPipe" : "error",
        "trackingPipe" : "error",
        "hires_large_colorPipe" : "error",
        "hires_large_greyPipe" : "error",
        "hires_large_encodedPipe" : "error",
        "hires_small_colorPipe" : "error",
        "hires_small_greyPipe" : "error",
        "hires_small_encodedPipe" : "error",
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-imu-server" : {
        "result" : False,
        "running" : False
    },
    "voxl-qvio-server" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixed" : "error",
        "mpaPipeLocal" : "error",
        "mpaPipeFixedPoseInput" : "error"
    }
}

_d0006_model = { # Sentinel
    "calOk" : False,
    "compute" : {
        "imageSensor0" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor1" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor2" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor3" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor4" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor5" : {
            "probe" : "error",
            "result" : False
        },
    },
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
        "stereo_front" : False,
        "stereo_rear" : False,
        "tracking" : False,
        "hires_large_color" : False,
        "hires_large_grey" : False,
        "hires_large_encoded" : False,
        "hires_small_color" : False,
        "hires_small_grey" : False,
        "hires_small_encoded" : False,
        "stereo_frontPipe" : "error",
        "stereo_rearPipe" : "error",
        "trackingPipe" : "error",
        "hires_large_colorPipe" : "error",
        "hires_large_greyPipe" : "error",
        "hires_large_encodedPipe" : "error",
        "hires_small_colorPipe" : "error",
        "hires_small_greyPipe" : "error",
        "hires_small_encodedPipe" : "error",
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-imu-server" : {
        "result" : False,
        "running" : False
    },
    "voxl-qvio-server" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixed" : "error",
        "mpaPipeLocal" : "error",
        "mpaPipeFixedPoseInput" : "error"
    }
}

_d0008_model = { # FPV
    "calOk" : False,
    "compute" : {},
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-imu-server" : {
        "result" : False,
        "running" : False
    },
    "voxl-open-vins-server" : {
        "result" : False,
        "running" : False,
        "ov" : "error",
        "ov_extended" : "error",
        "ov_overlay" : "error",
        "ov_status" : "error"
    },
    "voxl-lepton-server" : {
        "result" : False,
        "running" : False,
        "intrinsics" : "error",
        "mpaColor" : "error",
        "mpaRaw" : "error"
    },
    "voxl-lepton-tracker" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixedPoseInput" : "error"
    }
}

_d0012_model = { # Starling 2 Max
    "calOk" : False,
    "compute" : {},
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-imu-server" : {
        "result" : False,
        "running" : False
    },
    "voxl-open-vins-server" : {
        "result" : False,
        "running" : False,
        "ov" : "error",
        "ov_extended" : "error",
        "ov_overlay" : "error",
        "ov_status" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixed" : "error",
        "mpaPipeLocal" : "error",
        "mpaPipeFixedPoseInput" : "error"
    },
    "voxl-rangefinder-server" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    }
}

_d0013_model = { # Stinger
    "calOk" : False,
    "compute" : {
        "imageSensor0" : {
            "probe" : "error",
            "result" : False
        },
        "imageSensor1" : {
            "probe" : "error",
            "result" : False
        }
    },
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-imu-server" : {
        "result" : False,
        "running" : False
    },
    "voxl-open-vins-server" : {
        "result" : False,
        "running" : False,
        "ov" : "error",
        "ov_extended" : "error",
        "ov_overlay" : "error",
        "ov_status" : "error"
    },
    "voxl-lepton-server" : {
        "result" : False,
        "running" : False,
        "intrinsics" : "error",
        "mpaColor" : "error",
        "mpaRaw" : "error"
    },
    "voxl-lepton-tracker" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixed" : "error",
        "mpaPipeLocal" : "error",
        "mpaPipeFixedPoseInput" : "error"
    }
}

_d0014_model = { # Starling 2
    "calOk" : False,
    "compute" : {},
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-imu-server" : {
        "result" : False,
        "running" : False
    },
    "voxl-open-vins-server" : {
        "result" : False,
        "running" : False,
        "ov" : "error",
        "ov_extended" : "error",
        "ov_overlay" : "error",
        "ov_status" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixedPoseInput" : "error"
    },
    "voxl-rangefinder-server" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    }

}

_d0015_model = {
    "calOk" : False,
    "compute" : {
        "imageSensor0": {
            "probe": "error",
            "result": False
        }
    },
    "voxl-camera-server" : {
        "result" : False,
        "running" : False,
        "mpaOk" : False,
        "hires_large_color" : False,
        "hires_large_grey" : False,
        "hires_large_encoded" : False,
        "hires_small_color" : False,
        "hires_small_grey" : False,
        "hires_small_encoded" : False,
        "hires_large_colorPipe" : "error",
        "hires_large_greyPipe" : "error",
        "hires_large_encodedPipe" : "error",
        "hires_small_colorPipe" : "error",
        "hires_small_greyPipe" : "error",
        "hires_small_encodedPipe" : "error",
    },
    "voxl-mavlink-server" : {
        "result" : False,
        "running" : False,
    },
    "voxl-px4" : _px4_model,
    "voxl-imu-server" : {
        "result" : False,
        "running" : False
    },
    "voxl-qvio-server" : {
        "result" : False,
        "running" : False,
        "mpaPipe" : "error"
    },
    "voxl-vision-hub" : {
        "result" : False,
        "running" : False,
        "mpaPipeVoaPcOut" : "error",
        "mpaPipeFixed" : "error",
        "mpaPipeLocal" : "error",
        "mpaPipeFixedPoseInput" : "error"
    }

}

#
# Utils
#

def cleanup_logs(max_log_bytes): # clear /var/log
    disk_command = subprocess.check_output(["sudo","du","-h","/var/log/kern.log","/var/log/syslog"])
    lines = disk_command.decode().split('\n')
    display_value = 0
    total_size = 0

    for line in lines:
        parts = line.split()

        if len(parts) >= 2:
            value = parts[0] # size in GB
            unit = value[-1] # unit of value
            size_value = float(value[:-1])
            if unit == 'G':
                size_bytes = size_value * 1024**3 
            elif unit == 'M':
                size_bytes = size_value * 1024**2
            elif unit == 'K':
                size_bytes = size_value * 1024
            else:
                size_bytes = size_value

            total_size += size_bytes

    display_value = total_size / (1024**3)
    display_value_format = "{:.{}g}".format(display_value, 4)

    if total_size >= max_log_bytes:
        subprocess.run(["rm","-r","/var/log"])

    return {
        'size': display_value_format
    }

def parse_skus(sku):
    modem_config = None
    camera_config = None
    gimbal_config = None
    selected_model = None
    num_cams = 0
    gimbal_type = -1
    modem_type = 0
    exp_type = 0

    # Dict mapping SKU to model
    sku_to_model = {
        'd0004': _d0004_model,
        'd0005': _d0005_model,
        'd0006': _d0006_model,
        'd0008': _d0008_model,
        'd0012': _d0012_model,
        'd0013': _d0013_model,
        'd0014': _d0014_model,
        'd0015': _d0015_model
    }

    # Extract the model number from the SKU
    model_number = sku.split('-')[1]

    # Formulate the model name
    model_name = f"{model_number.lower()}"

    # Check if the model name exists in dict
    if model_name in sku_to_model:
        selected_model = sku_to_model[model_name]
    else:
        print("Error: Model not found")

    # split up sku 
    sku = sku.split('-')
    
    if selected_model is not None:
        for type in sku:
            # Camera configs
            if type.startswith('C') and type[1:].isdigit():
                camera_config = type[1:] # grab camera configs
                if camera_config == '15':
                    for i in range(2):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["compute"]["i2c0"] = {                                                                            
                        "probe": "",
                        "result": False
                    }
                    selected_model["voxl-camera-server"]["trackingL_grey"] = False
                    selected_model["voxl-camera-server"]["trackingL_color"] = False
                    selected_model["voxl-camera-server"]["trackingR_grey"] = False
                    selected_model["voxl-camera-server"]["trackingR_color"] = False
                    selected_model["voxl-camera-server"]["trackingL_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["trackingL_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["trackingR_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["trackingR_colorPipe"] = "error"
                    num_cams = 2

                elif camera_config == '18' or camera_config == '32':
                    for i in range(3):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["trackingL_grey"] = False
                    selected_model["voxl-camera-server"]["trackingL_color"] = False
                    selected_model["voxl-camera-server"]["trackingR_grey"] = False
                    selected_model["voxl-camera-server"]["trackingR_color"] = False
                    selected_model["voxl-camera-server"]["hires_color"] = False
                    selected_model["voxl-camera-server"]["hires_grey"] = False
                    selected_model["voxl-camera-server"]["trackingL_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["trackingL_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["trackingR_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["trackingR_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_greyPipe"] = "error"
                    selected_model["compute"]["i2c0"] = {
                        "probe": "",
                        "result": False
                    }
                    num_cams = 3
                                                
                elif camera_config == '22': # in platform_config_2 for voxl 2 mini
                    for i in range(2):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_down_misp_grey"] = False
                    selected_model["voxl-camera-server"]["tracking_down_misp_norm"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["tracking_front_misp_grey"] = False
                    selected_model["voxl-camera-server"]["tracking_front_misp_norm"] = False
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_down_misp_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_down_misp_normPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_front_misp_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_down_misp_normPipe"] = "error"
                    num_cams = 2
                
                elif camera_config == '26':
                    for i in range(4):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["hires_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_small_grey"] = False
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["tof_conf"] = False
                    selected_model["voxl-camera-server"]["tof_depth"] = False
                    selected_model["voxl-camera-server"]["tof_ir"] = False
                    selected_model["voxl-camera-server"]["tof_pc"] = False
                    selected_model["voxl-camera-server"]["hires_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_confPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_depthPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_irPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_pcPipe"] = "error"
                    
                    num_cams = 4

                elif camera_config == '27':
                    for i in range(5):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["hires_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_small_grey"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_rear"] = False
                    selected_model["voxl-camera-server"]["tof_conf"] = False
                    selected_model["voxl-camera-server"]["tof_depth"] = False
                    selected_model["voxl-camera-server"]["tof_ir"] = False
                    selected_model["voxl-camera-server"]["tof_pc"] = False
                    selected_model["voxl-camera-server"]["hires_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_rearPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_confPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_depthPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_irPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_pcPipe"] = "error"
                    num_cams = 5
                
                elif camera_config == '28':
                    for i in range(4):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["hires_down_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_grey"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_grey"] = False
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    num_cams = 4
                
                elif camera_config == '29':
                    for i in range(5):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["hires_down_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_grey"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_grey"] = False
                    selected_model["voxl-camera-server"]["tof_conf"] = False
                    selected_model["voxl-camera-server"]["tof_depth"] = False
                    selected_model["voxl-camera-server"]["tof_ir"] = False
                    selected_model["voxl-camera-server"]["tof_pc"] = False
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_confPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_depthPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_irPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_pcPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    num_cams = 5
                
                elif camera_config == '30': # need to test / validate
                    for i in range(6):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_down_small_grey"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_front_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_front_small_grey"] = False
                    selected_model["voxl-camera-server"]["tof_conf"] = False
                    selected_model["voxl-camera-server"]["tof_depth"] = False
                    selected_model["voxl-camera-server"]["tof_ir"] = False
                    selected_model["voxl-camera-server"]["tof_pc"] = False
                    selected_model["voxl-camera-server"]["tof2_conf"] = False
                    selected_model["voxl-camera-server"]["tof2_depth"] = False
                    selected_model["voxl-camera-server"]["tof2_ir"] = False
                    selected_model["voxl-camera-server"]["tof2_pc"] = False
                    selected_model["voxl-camera-server"]["hires_down_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_down_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_front_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_confPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_depthPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_irPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof_pcPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof2_confPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof2_depthPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof2_irPipe"] = "error"
                    selected_model["voxl-camera-server"]["tof2_pcPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    num_cams = 6

                elif camera_config == '36':
                    for i in range(3):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["hires_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_small_grey"] = False
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_greyPipe"] = "error"
                    num_cams = 3

                elif camera_config == '37': # Need to test / validate
                    for i in range(4):
                        selected_model["compute"][f"imageSensor{i}"] = {
                            "probe": "error",
                            "result": False
                        }
                    selected_model["voxl-camera-server"]["tracking_down"] = False
                    selected_model["voxl-camera-server"]["tracking_front"] = False
                    selected_model["voxl-camera-server"]["hires_large_color"] = False
                    selected_model["voxl-camera-server"]["hires_large_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_large_grey"] = False
                    selected_model["voxl-camera-server"]["hires_small_color"] = False
                    selected_model["voxl-camera-server"]["hires_small_encoded"] = False
                    selected_model["voxl-camera-server"]["hires_small_grey"] = False
                    selected_model["voxl-camera-server"]["boson"] = False
                    selected_model["voxl-camera-server"]["tracking_downPipe"] = "error"
                    selected_model["voxl-camera-server"]["tracking_frontPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_large_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_colorPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_encodedPipe"] = "error"
                    selected_model["voxl-camera-server"]["hires_small_greyPipe"] = "error"
                    selected_model["voxl-camera-server"]["bosonPipe"] = "error"
                    num_cams = 4

            # Experimental configs
            elif type.startswith('E') and len(type) > 1 and type[1] != '0' and type[1:].isdigit():
                exp_config = type[1:] # grab experimental config
                if exp_config == '8':
                    exp_type = 8

            # Modem configs
            elif type.startswith('M') and len(type) > 1 and type[1] != '0' and type[1:].isdigit():
                modem_config = type[1:]  # grab modem config
                selected_model["voxl-modem"] = {}
                selected_model["voxl-modem"] = {
                    "result": False,
                    "enum": False,
                    "inet": "error"
                }
                modem_type = int(modem_config)


            # Gimbal configs
            elif type.startswith('X') and type[1:].isdigit():
                gimbal_config = type[1:] # grab gimbal configs
                if gimbal_config == '0':
                    continue
                elif gimbal_config == '1':
                    # something with CADDX
                    gimbal_type = 1
                elif gimbal_config == '2':
                    selected_model["voxl-uvc-server"] = {}
                    selected_model["voxl-uvc-server"] = {
                        "result": False,
                        "running": False,
                        "device": "",
                        "mpaPipe": "error"
                    }
                    gimbal_type = 2
                elif gimbal_config == '4':
                    selected_model["voxl-lepton-server"] = {}
                    selected_model["voxl-lepton-server"] = {
                        "result" : False,
                        "running" : False,
                        "intrinsics" : "error",
                        "mpaColor" : "error",
                        "mpaRaw" : "error",
                    }
                    gimbal_type = 4
                elif gimbal_config == '8': # FLIR Lepton
                    selected_model["voxl-lepton-server"] = {}
                    selected_model["voxl-lepton-server"] = {
                        "result" : False,
                        "running" : False,
                        "intrinsics" : "error",
                        "mpaColor" : "error",
                        "mpaRaw" : "error",
                    }
                    gimbal_type = 8
    else:
        print("Error: selected_model is not assigned")
        return 0, -1, 0, 0
    
    return num_cams, gimbal_type, modem_type, camera_config, exp_type
    

def parse_args():
    """
    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('-j', '--json', help='print output as json', required=False, action='store_true')
    parser.add_argument('-r', '--enable-rc', help='Enable radio receiver', required=False, action='store_true')
    args = vars(parser.parse_args())

    return args



def print_usage():
    """
    Print usage/help message
    """
    print("Description: Print a health report of this target.")
    print("")
    print("Optional Arguments:")
    print(" -h, --help   Show this help message and exit")
    print(" -j, --json   print json instead of human readable output")
    print(" -r, --enable-rc Enable radio receiver ")
    print("")


def print_readable(resp: dict):
    """
    Function to output result of health check in a human readable format

    @resp: A JSON object containing results from the health check
    """
    health_check_result = f"{GRN}Pass{RESET}" if resp["healthCheckResult"] else f"{RED}Fail{RESET}"
    test_check = True

    if not args["json"]:
        print(f"{BOLD}SKU: {resp['sku']}", end="")

    # Print calOk
        if resp["calOk"]:
            print(f"\n{GRN}>> calOk: True")
        else:
            print(f"\n{RED}>> calOk: False")

    # Top level - voxl-services
    for service in resp:
        # Skip UID, factory mode, health check result
        if type(resp[service]) != dict:
            continue

        # Show deep results and type
        if "result" in list(resp[service].keys()):
            if resp[service]["result"] == True:
                if not args["json"]:
                    print(f"\n{GRN}>> {service}:{RESET}")
            else:
                if not args["json"]:
                    print(f"\n{RED}>> {service}:{RESET}")
                test_check = False
        else:
            color = GRN
            for imageSensor in resp[service]:
                if resp[service][imageSensor]["result"] == False:
                    color = RED
                    test_check = False
            if not args["json"]:
                print(f"\n{color}>> {service}:{RESET}")

        # Test level - results from service tests
        for test, result in resp[service].items():
            color = GRN

            # If not sub nested dict, print test and result
            if type(result) != dict:
                if resp[service][test] == False or resp[service][test] == "False" or resp[service][test] == "error" or resp[service][test] == "":
                    if test == "error" and resp[service][test] == False:
                        color = GRN
                    else:
                        color = RED
                        test_check = False

                # Error == True -> RED... normal logic here makes error == true print as green...
                if test == "error" and resp[service][test] == True:
                    color = RED
                    test_check == False

                if not args["json"]:
                    print(f"\t{color}> {test}: {result}{RESET}")
                continue
            
            # Otherwise, print new dict and its type
            result_val = resp[service][test].get("result", False)
            if result_val == False:
                color = RED
                test_check = False
            
            if not args["json"]:
                print(f"\t{color}> {test}{RESET}")

            # Sub test level - results from sensor tests
            for subtest, subtest_result in result.items():
                if type(subtest_result) != dict:
                    color = GRN
                    if resp[service][test][subtest] == False or resp[service][test][subtest] == "False" or resp[service][test][subtest] == "error" or resp[service][test][subtest] == "":
                        if subtest == "vel_m_s" or subtest == "satellites_used" or subtest == "lon" or subtest == "lat":
                            color = GRN
                        else:
                            color = RED
                            test_check = False

                    if resp[service][test][subtest] == -1 or str(resp[service][test][subtest]).strip() == "nan":
                        color = RED
                        test_check = False
                    
                    if not args["json"]:
                        print(f"\t\t{color}{subtest}: {subtest_result}{RESET}")
                else:
                    print("Need to update parsing logic for another level deeper!")

    health_check_result = f"{GRN}Pass{RESET}" if test_check else f"{RED}Fail{RESET}"
    if not args["json"]:
        print(f"\n{BOLD}Health Check: {health_check_result}{RESET}")
    return test_check


def read_file_line(filename):
    """
    reads a line from a file

    :param filename: path of file to read
    :return: tuple, boolean flag and file content
    """ 
    if not os.path.exists(filename):
        print(f"File not found: {filename}")
        return False, None
    
    try:
        with open(filename) as f:
            content = f.read().strip()
            return True, content
    except Exception as e:
        print(f"Error reading file: {e}")
        return False, None


def get_value_from_msg(msg, variable, status=False, default=None):
    """
    Gets a value based on variable in a message
    :param msg: message to parse
    :param variable: variable to search for in message
    """
    data = msg.split("\n")
    for line in data:
        # For when trying to get service status
        if status and variable in line:
            val = line.split('|')
            if val[0].strip(" ") == variable:
                return val
        # For when trying to get data from sensor
        elif variable + ":" in line:
            val = line.split(':')[1]
            return val
    return default


def get_exec_start(service_file_path):
    """
    Checks the method used to start a service

    :param service_file_path: path of service file to check
    """
    with open(service_file_path) as f:
        for line in f:
            if line.startswith("ExecStart="):
                return line.strip()[len("ExecStart="):]
    return None


def is_service_running(service_name):
    """
    Checks if a systemd service is running or not

    :param service_name: name of service to check
    """
    cmd = ['systemctl', 'is-active', service_name]
    result = subprocess.run(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    if result.returncode == 0:
        return True
    else:
        return False


#
# Sensor tests
#
    
def test_accel(resp):
    """
    Tests accel values

    param resp: full JSON data model of test target
    """

    accel_limits = {
        "X": (-1.0, 1.0),
        "Y": (-1.0, 1.0),
        "Z": (-10.6, -9.0)
    }

    result = all(
        accel_limits[axis][0] <= resp["voxl-px4"]["sensor_accel"][f"{axis.lower()}"] <= accel_limits[axis][1]
        for axis in ["X", "Y", "Z"]
    )

    return result


def test_gyro(resp):
    """
    Tests gyro values

    param resp: full JSON data model of test target
    """

    gyro_limits = {
        "X": (-0.5, 0.5),
        "Y": (-0.5, 0.5),
        "Z": (-0.5, 0.5)
    }

    result = all(
        gyro_limits[axis][0] <= resp["voxl-px4"]["sensor_gyro"][f"{axis.lower()}"] <= gyro_limits[axis][1]
        for axis in ["X", "Y", "Z"]
    )

    return result


def test_baro(resp):
    """
    Tests baro values

    param resp: full JSON data model of test target
    """
    baro_press_min = 95000.0
    baro_press_max = 105000.0
    baro_temp_min = 20.0
    baro_temp_max= 60.0
    baro_press = resp["voxl-px4"]["sensor_baro"]["pressure"]
    baro_temp = resp["voxl-px4"]["sensor_baro"]["temperature"]

    result = True

    # Baro pressure check
    if baro_press < baro_press_min or baro_press > baro_press_max:
        # print("Baro pressure out of bounds")
        result = False

    # Baro temperature check
    if baro_temp < baro_temp_min or baro_temp > baro_temp_max:
        # print("Baro temp out of bounds")
        result = False

    return result


def test_mag(resp):
    """
    Tests mag values

    param resp: full JSON data model of test target
    """

    mag_limits = {
        "X": (-1.25, 1.25),
        "Y": (-1.25, 1.25),
        "Z": (-1.25, 1.25)
    }

    result = all(
        mag_limits[axis][0] <= resp["voxl-px4"]["sensor_mag"][f"{axis.lower()}"] <= mag_limits[axis][1]
        for axis in ["X", "Y", "Z"]
    )

    return result


def test_gps(resp, sample):
    """
    Tests gps values

    param resp: full JSON data model of test target
    """
    result = True

    # Check that topic is published
    if "not published" in sample:
        result = False

    return result


def test_battery(resp):
    """
    Tests battery values

    param resp: full JSON data model of test target
    """
    voltage_min = 0
    voltage_max = 25.2
    voltage = resp["voxl-px4"]["battery_status"]["voltage_filtered_v"]

    result = True

    # Check that voltage is in acceptable range (max for D0008 is 25.2V at full battery I believe)
    if voltage <= voltage_min or voltage >= voltage_max:
        result = False

    return result


def test_rc(resp):
    """
    Tests rc values

    param resp: full JSON data model of test target
    """
    result = True
    rc_connection = resp["voxl-px4"]["input_rc"]["rc_connection"]

    # Check that we have an rc_connection
    if rc_connection.strip() == "False" or rc_connection == "":
        result = False

    return result

def sensor_retry(sensor_name, sensor_data_func, test_func, resp, max_tries=10):
    resp[f"voxl-px4"][sensor_name] = sensor_data_func()
    resp[f"voxl-px4"][sensor_name]["result"] = test_func(resp)
    if not resp[f"voxl-px4"][sensor_name]["result"]:
        resp[f"voxl-px4"][sensor_name]["result"] = False
        resp["voxl-px4"]["result"] = False
        return False
    else:
        return True

#
# Service Tests
#

def voxl_camera_server_health_check(resp, assembly_sku, dmesg_lines):

    if "D0004" in assembly_sku:
        result = True
        #
        # Platform Checks
        #

        # check that sensors were probed
        cam_success_configs = [
            "Probe success,slot:0,slave_addr:0xe2,sensor_id:0x7750",
            "Probe success,slot:1,slave_addr:0xe4,sensor_id:0x7750",
            "Probe success,slot:2,slave_addr:0xe2,sensor_id:0x7750",
            "Probe success,slot:3,slave_addr:0x20,sensor_id:0x214",
            "Probe success,slot:4,slave_addr:0xe2,sensor_id:0x7750",
            "Probe success,slot:5,slave_addr:0xe4,sensor_id:0x7750"
        ]

        for i, line in enumerate(dmesg_lines):
            for cam_num, cam_success in enumerate(cam_success_configs):
                key = f"imageSensor{cam_num}"
                if cam_success in line:
                    resp["compute"][key]["probe"] = cam_success
                    resp["compute"][key]["result"] = True
        
        if any("error" in resp["compute"][f"imageSensor{i}"]["probe"] for i in range(len(cam_success_configs))):
            result = False


        #
        # SDK checks
        #

        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True

        resp["voxl-camera-server"]["mpaOk"] = True

        camera_types = {
            "stereo_front", "stereo_rear", "tracking", "hires_large_color", "hires_large_grey", "hires_large_encoded",
            "hires_small_color", "hires_small_grey", "hires_small_encoded"
        }

        for cam in camera_types:
            pipe_path = f"/run/mpa/{cam}/info"
            if path.exists(pipe_path):
                output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                if not output.returncode:
                    resp["voxl-camera-server"][f"{cam}"] = True
                    resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                else:
                    resp["voxl-camera-server"]["mpaOk"] = False
                    result = False

        resp["voxl-camera-server"]["result"] = result


    elif "D0005" in assembly_sku:
        result = True

        #
        # Platform Checks
        #

        # check that sensors were probed
        cam_success_configs = [
            "CAM_START_DEV Success, sensor_id:0x3d,sensor_slave_addr:0x7a",
            "CAM_START_DEV Success, sensor_id:0x214,sensor_slave_addr:0x20",
            "CAM_START_DEV Success, sensor_id:0x7750,sensor_slave_addr:0xe2"
        ]

        for i, line in enumerate(dmesg_lines):
            for cam_num, cam_success in enumerate(cam_success_configs):
                key = f"imageSensor{cam_num}"
                if cam_success in line:
                    resp["compute"][key]["probe"] = cam_success
                    resp["compute"][key]["result"] = True

        if any("error" in resp["compute"][f"imageSensor{i}"]["probe"] for i in range(len(cam_success_configs))):
            result = False

        #
        # SDK Checks
        #
        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True
        
        resp["voxl-camera-server"]["mpaOk"] = True

        camera_types = {
            "tof_conf", "tof_depth", "tof_ir", "tracking", "hires_large_color", "hires_large_grey", "hires_large_encoded",
            "hires_small_color", "hires_small_grey", "hires_small_encoded"
        }

        for cam in camera_types:
            pipe_path = f"run/mpa/{cam}/info"
            if path.exists(pipe_path):
                output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                if not output.returncode:
                    resp["voxl-camera-server"][f"{cam}"] = True
                    resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                else:
                    resp["voxl-camera-server"]["mpaOk"] = False
                    result = False

        if path.exists("/run/mpa/tof_pc/info"):
            resp["voxl-camera-server"]["tof_pc"] = True
            resp["voxl-camera-server"]["tof_pcPipe"] = "/run/mpa/tof_pc/info"
        else:
            resp["voxl-camera-server"]["mpaOk"] = False
            result = False

        resp["voxl-camera-server"]["result"] = result

    elif "D0006" in assembly_sku:
        result = True

        #
        # Platform Checks
        #
        cam_success_configs = [
            "Probe success,slot:0,slave_addr:0xe2,sensor_id:0x7750",
            "Probe success,slot:1,slave_addr:0xe4,sensor_id:0x7750",
            "Probe success,slot:2,slave_addr:0xe2,sensor_id:0x7750",
            "Probe success,slot:3,slave_addr:0x20,sensor_id:0x214",
            "Probe success,slot:4,slave_addr:0xe2,sensor_id:0x7750",
            "Probe success,slot:5,slave_addr:0xe4,sensor_id:0x7750"
        ]

        for i, line in enumerate(dmesg_lines):
            for cam_num, cam_success in enumerate(cam_success_configs):
                key = f"imageSensor{cam_num}"
                if cam_success in line:
                    resp["compute"][key]["probe"] = cam_success
                    resp["compute"][key]["result"] = True
        
        if any("error" in resp["compute"][f"imageSensor{i}"]["probe"] for i in range(len(cam_success_configs))):
            result = False
        
        #
        # SDK Checks
        #
        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True
        resp["voxl-camera-server"]["mpaOk"] = True

        camera_types = {
            "stereo_front", "stereo_rear", "hires_large_color", "hires_large_grey", "hires_large_encoded", 
            "hires_small_color", "hires_small_grey", "hires_small_encoded", "tracking"
        }

        for cam in camera_types:
            pipe_path = f"/run/mpa/{cam}/info"
            if path.exists(pipe_path):
                output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                if not output.returncode:
                    resp["voxl-camera-server"][f"{cam}"] = True
                    resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                else:
                    resp["voxl-camera-server"]["mpaOk"] = False
                    result = False

        resp["voxl-camera-server"]["result"] = result

    elif "D0008" in assembly_sku:
        result = True

        num_cams, _, _, camera_config, _ = parse_skus(resp["sku"])

        #
        # Platform Checks
        #
        # check that senors were probed
        c15_success_configs = [
            "Probe success,slot:0,slave_addr:0xe2",
            "Probe success,slot:1,slave_addr:0xe4"
        ]

        c32_success_configs = [
            "Probe success,slot:0,slave_addr:0xe2",
            "Probe success,slot:1,slave_addr:0xe4",
            "Probe success,slot:5,slave_addr:0xe4"
        ]

        c36_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:1,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
        ]

        for line in dmesg_lines:
            # C15
            if camera_config == '15':
                for cam_num, cam_success in enumerate(c15_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True
            # C32
            elif camera_config == '32':
                for cam_num, cam_success in enumerate(c32_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True
            # C36
            elif camera_config == '36':
                for cam_num, cam_success in enumerate(c36_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True

        #error checking
        for i in range(num_cams):
            if not "error" in resp["compute"][f"imageSensor{i}"]["probe"]:
                resp["compute"][f"imageSensor{i}"]["result"] = True
            else:
                result = False

        #
        # SDK Checks
        #
        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True
            resp["voxl-camera-server"]["mpaOk"] = True

        camera_list = {
            'c15': {
                "trackingL_grey", "trackingL_color", "trackingR_grey", "trackingR_color"
            },
            'c32': {
                "trackingL_grey", "trackingL_color", "trackingR_grey", "trackingR_color",
                "hires_color", "hires_grey"
            },
            'c36': {
                "hires_large_color", "hires_large_grey", "hires_large_encoded",
                "hires_small_color", "hires_small_grey", "hires_small_encoded",
                "tracking_front", "tracking_down"
            }
        }

        for i, camera_types in camera_list.items():
            for cam in camera_types:
                pipe_path = f"run/mpa/{cam}/info"
                if path.exists(pipe_path):
                    output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                    if not output.returncode:
                        resp["voxl-camera-server"][f"{cam}"] = True
                        resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                    else:
                        resp["voxl-camera-server"]["mpaOk"] = False
                        result = False

        resp["voxl-camera-server"]["result"] = result

    elif "D0012" in assembly_sku:
        result = True

        num_cams, _, _, camera_config, _ = parse_skus(resp["sku"])

        c28_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:1,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:2,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
        ]
        c29_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:1,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:2,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:3,slave_addr:0x7a,sensor_id:0x2975",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
        ]

        for line in dmesg_lines:
            # C28
            if camera_config == '28':
                for cam_num, cam_success in enumerate(c28_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True
            # C29
            elif camera_config == '29':
                for cam_num, cam_success in enumerate(c29_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True

        #error checking
        for i in range(num_cams):
            if not "error" in resp["compute"][f"imageSensor{i}"]["probe"]:
                resp["compute"][f"imageSensor{i}"]["result"] = True
            else:
                result = False
        
        #
        # SDK Checks
        #
        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True
            resp["voxl-camera-server"]["mpaOk"] = True

            camera_types = {
                "hires_down_large_color", "hires_down_large_encoded", "hires_down_large_grey", "hires_down_small_color", "hires_down_small_encoded", "hires_down_small_grey",
                "hires_front_large_color", "hires_front_large_encoded", "hires_front_large_grey", "hires_front_small_color", "hires_front_small_encoded", "hires_front_small_grey",
                "tracking_down", "tracking_front"
            }
            camera_list = {
                'c28': {
                    "hires_down_large_color", "hires_down_large_encoded", "hires_down_large_grey", "hires_down_small_color", "hires_down_small_encoded", "hires_down_small_grey",
                    "hires_front_large_color", "hires_front_large_encoded", "hires_front_large_grey", "hires_front_small_color", "hires_front_small_encoded", "hires_front_small_grey",
                    "tracking_down", "tracking_front"
                },
                'c29': {
                    "hires_down_large_color", "hires_down_large_encoded", "hires_down_large_grey", "hires_down_small_color", "hires_down_small_encoded", "hires_down_small_grey",
                    "hires_front_large_color", "hires_front_large_encoded", "hires_front_large_grey", "hires_front_small_color", "hires_front_small_encoded", "hires_front_small_grey",
                    "tof_conf", "tof_depth", "tof_ir",
                    "tracking_down", "tracking_front"
                }
            }

        for i, camera_types in camera_list.items():
            for cam in camera_types:
                pipe_path = f"run/mpa/{cam}/info"
                if path.exists(pipe_path):
                    output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                    if not output.returncode:
                        resp["voxl-camera-server"][f"{cam}"] = True
                        resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                    else:
                        resp["voxl-camera-server"]["mpaOk"] = False
                        result = False
        if path.exists("/run/mpa/tof_pc/info"):
            resp["voxl-camera-server"]["tof_pc"] = True
            resp["voxl-camera-server"]["tof_pcPipe"] = "/run/mpa/tof_pc/info"

        resp["voxl-camera-server"]["result"] = result

    elif "D0013" in assembly_sku:
        result = True

        num_cams, _, _, camera_config, _ = parse_skus(resp["sku"])

        c22_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
            # "CAM_ACQUIRE_DEV Success, sensor_id:0x356,sensor_slave_addr:0x30",
            # "CAM_ACQUIRE_DEV Success, sensor_id:0x356,sensor_slave_addr:0x30"
        ]

        for line in dmesg_lines:
            if camera_config == '22':
                for cam_num, cam_success in enumerate(c22_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True

        #error checking
        for i in range(num_cams):
            if not "error" in resp["compute"][f"imageSensor{i}"]["probe"]:
                resp["compute"][f"imageSensor{i}"]["result"] = True
            else:
                result = False

        #
        # SDK Checks
        #
        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True
            resp["voxl-camera-server"]["mpaOk"] = True

        camera_list = {
            'c22': {
                "tracking_down", "tracking_down_misp_grey", "tracking_down_misp_norm",
                "tracking_front", "tracking_front_misp_grey", "tracking_front_misp_norm"
            }
        }

        for i, camera_types in camera_list.items():
            for cam in camera_types:
                pipe_path = f"run/mpa/{cam}/info"
                if path.exists(pipe_path):
                    output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                    if not output.returncode:
                        resp["voxl-camera-server"][f"{cam}"] = True
                        resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                    else:
                        resp["voxl-camera-server"]["mpaOk"] = False
                        result = False
        
        resp["voxl-camera-server"]["result"] = result

    elif "D0014" in assembly_sku:
        result = True

        num_cams, _, _, camera_config, _ = parse_skus(resp["sku"])

        c26_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:1,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:3,slave_addr:0x7a,sensor_id:0x2975",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
        ]

        c27_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:1,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:2,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:3,slave_addr:0x7a,sensor_id:0x2975",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
        ]

        c28_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:1,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:2,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
        ]

        c29_success_configs = [
            "Probe success,slot:0,slave_addr:0x30,sensor_id:0x356",
            "Probe success,slot:1,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:2,slave_addr:0x34,sensor_id:0x577",
            "Probe success,slot:3,slave_addr:0x7a,sensor_id:0x2975",
            "Probe success,slot:6,slave_addr:0x30,sensor_id:0x356"
        ]

        for line in dmesg_lines:
            # C26
            if camera_config == '26':
                for cam_num, cam_success in enumerate(c26_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True
            # C27
            elif camera_config == '27':
                for cam_num, cam_success in enumerate(c27_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True
            # C28
            elif camera_config == '28':
                for cam_num, cam_success in enumerate(c28_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True
            # C29
            elif camera_config == '29':
                for cam_num, cam_success in enumerate(c29_success_configs):
                    key = f"imageSensor{cam_num}"
                    if cam_success in line:
                        resp["compute"][key]["probe"] = cam_success
                        resp["compute"][key]["result"] = True
        
        #error checking
        for i in range(num_cams):
            if not "error" in resp["compute"][f"imageSensor{i}"]["probe"]:
                resp["compute"][f"imageSensor{i}"]["result"] = True
            else:
                result = False
        
        #
        # SDK Checks
        #
        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True
            resp["voxl-camera-server"]["mpaOk"] = True

        camera_list = {
            'c26': {
                "hires_large_color", "hires_large_grey", "hires_large_encoded",
                "hires_small_color", "hires_small_grey", "hires_small_encoded",
                "tracking_front", "tracking_down",
                "tof_conf", "tof_depth", "tof_ir"
            },
            'c27': {
                "hires_large_color", "hires_large_grey", "hires_large_encoded",
                "hires_small_color", "hires_small_grey", "hires_small_encoded",
                "tof_conf", "tof_depth", "tof_ir",
                "tracking_front", "tracking_down", "tracking_rear"
            },
            'c28': {
                "hires_down_large_color", "hires_down_large_encoded", "hires_down_large_grey", "hires_down_small_color", "hires_down_small_encoded", "hires_down_small_grey",
                "hires_front_large_color", "hires_front_large_encoded", "hires_front_large_grey", "hires_front_small_color", "hires_front_small_encoded", "hires_front_small_grey",
                "tracking_down", "tracking_front"
            },
            'c29': {
                "hires_down_large_color", "hires_down_large_encoded", "hires_down_large_grey", "hires_down_small_color", "hires_down_small_encoded", "hires_down_small_grey",
                "hires_front_large_color", "hires_front_large_encoded", "hires_front_large_grey", "hires_front_small_color", "hires_front_small_encoded", "hires_front_small_grey",
                "tof_conf", "tof_depth", "tof_ir",
                "tracking_down", "tracking_front"
            }
        }

        for i, camera_types in camera_list.items():
            for cam in camera_types:
                pipe_path = f"run/mpa/{cam}/info"
                if path.exists(pipe_path):
                    output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                    if not output.returncode:
                        resp["voxl-camera-server"][f"{cam}"] = True
                        resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                    else:
                        resp["voxl-camera-server"]["mpaOk"] = False
                        result = False

        if path.exists("/run/mpa/tof_pc/info"):
            resp["voxl-camera-server"]["tof_pc"] = True
            resp["voxl-camera-server"]["tof_pcPipe"] = "/run/mpa/tof_pc/info"

        resp["voxl-camera-server"]["result"] = result

    elif "D0015" in assembly_sku:
        result = True

        #
        # Platform Checks
        #
        # check that senors were probed
        cam_success_configs = [
            "Probe success,slot:2,slave_addr:0x34,sensor_id:0x577"
        ]

        for i, line in enumerate(dmesg_lines):
            for cam_num, cam_success in enumerate(cam_success_configs):
                key = f"imageSensor{cam_num}"
                if cam_success in line:
                    resp["compute"][key]["probe"] = cam_success
                    resp["compute"][key]["result"] = True
        
        #error checking
        if any("error" in resp["compute"][f"imageSensor{i}"]["probe"] for i in range(len(cam_success_configs))):
            result = False
        
        #
        # SDK Checks
        #
        if is_service_running("voxl-camera-server"):
            resp["voxl-camera-server"]["running"] = True
            resp["voxl-camera-server"]["mpaOk"] = True

            camera_types = {
                "hires_large_color", "hires_large_grey", "hires_large_encoded", 
                "hires_small_color", "hires_small_grey", "hires_small_encoded" 
            }

            for cam in camera_types:
                pipe_path = f"/run/mpa/{cam}/info"
                if path.exists(pipe_path):
                    output = subprocess.run(f"voxl-inspect-cam {cam} -t".split(), stdout = subprocess.DEVNULL)
                    if not output.returncode:
                        resp["voxl-camera-server"][f"{cam}"] = True
                        resp["voxl-camera-server"][f"{cam}Pipe"] = pipe_path
                    else:
                        resp["voxl-camera-server"]["mpaOk"] = False
                        result = False
        else:
            result = False

        resp["voxl-camera-server"]["result"] = result


def voxl_px4_factory_test(resp, assembly_sku, enable_rc):
    """

    param resp: full JSON data model of test target
    param assembly_sku: factory mode string
    """

    if "D0004" in assembly_sku or "D0005" in assembly_sku or "D0006" in assembly_sku or "D0008" in assembly_sku or "D0012" in assembly_sku or "D0013" in assembly_sku or "D0014" in assembly_sku or "D0015" in assembly_sku:

        #
        # SDK checks
        #
        if is_service_running("voxl-px4"):
            resp["voxl-px4"]["result"] = True
            resp["voxl-px4"]["running"] = True
            resp["voxl-px4"]["execStart"] = get_exec_start(VOXL_PX4_SERVICE_FILE)

            TOPIC_NAME_POSITION = 3

            listener_command = ['px4-listener', '--instance', '0', '-n', '1']

            get_commander_state = listener_command[:]
            get_accel = listener_command[:]
            get_gyro = listener_command[:]
            get_magnetometer = listener_command[:]
            get_gps = listener_command[:]
            get_barometer = listener_command[:]
            get_battery_status = listener_command[:]
            get_rc_input = listener_command[:]

            get_commander_state.insert(TOPIC_NAME_POSITION, 'commander_state')

            command_loop = []
            get_accel.insert(TOPIC_NAME_POSITION, 'sensor_accel')
            command_loop.append(get_accel)
            get_gyro.insert(TOPIC_NAME_POSITION, 'sensor_gyro')
            command_loop.append(get_gyro)
            get_magnetometer.insert(TOPIC_NAME_POSITION, 'sensor_mag')
            command_loop.append(get_magnetometer)
            get_barometer.insert(TOPIC_NAME_POSITION, 'sensor_baro')
            command_loop.append(get_barometer)
            get_battery_status.insert(TOPIC_NAME_POSITION, 'battery_status')
            command_loop.append(get_battery_status)

            if "D0013" not in assembly_sku:
                _px4_model["sensor_mag"] = {}
                _px4_model["sensor_gps"] = {}
                _px4_model["sensor_gps"] = {
                    "result" : False,
                    "lon" : 0.0,
                    "lat" : 0.0,
                    "alt" : 0.0,
                    "vel_m_s" : 0.0,
                    "satellites_used" : 0.0,
                    "device_id" : ""
                }
                _px4_model["sensor_mag"] = {
                    "result" : False,
                    "x" : 0.0,
                    "y" : 0.0,
                    "z" : 0.0
                }
                get_magnetometer.insert(TOPIC_NAME_POSITION, 'sensor_mag')
                command_loop.append(get_magnetometer)
                get_gps.insert(TOPIC_NAME_POSITION, 'sensor_gps')
                command_loop.append(get_gps)

            if enable_rc == True:
                get_rc_input.insert(TOPIC_NAME_POSITION, 'input_rc')
                command_loop.append(get_rc_input)

            try:
                for cmd in command_loop:
                    px4_tries = 0
                    while px4_tries < 10:
                        sample = subprocess.check_output(cmd).decode("utf-8")
                        if "timestamp" not in sample:
                            px4_tries+=1
                            time.sleep(1)
                        else:
                            break
                    else:
                        continue
                
                    if "timestamp" in sample:
                        if "sensor_accel" in sample:
                            sensor_retry("sensor_accel",
                                         lambda: {
                                            "x" : float(get_value_from_msg(sample, "x")),
                                            "y" : float(get_value_from_msg(sample, "y")),
                                            "z" : float(get_value_from_msg(sample, "z"))
                                         },
                                         test_accel, resp)
                            
                        elif "sensor_gyro" in sample:
                            sensor_retry("sensor_gyro",
                                         lambda: {
                                            "x" : float(get_value_from_msg(sample, "x")),
                                            "y" : float(get_value_from_msg(sample, "y")),
                                            "z" : float(get_value_from_msg(sample, "z"))
                                         },
                                         test_gyro, resp)
                            
                        elif "sensor_mag" in sample:
                            sensor_retry("sensor_mag",
                                         lambda: {
                                            "x" : float(get_value_from_msg(sample, "x")),
                                            "y" : float(get_value_from_msg(sample, "y")),
                                            "z" : float(get_value_from_msg(sample, "z"))
                                         },
                                         test_mag, resp)
                        
                        elif "sensor_gps" in sample:
                            sensor_retry("sensor_gps",
                                         lambda: {
                                            "lon" : float(get_value_from_msg(sample, "lon", default=0.00)),
                                            "lat" : float(get_value_from_msg(sample, "lat", default=0.00)),
                                            "alt" : float(get_value_from_msg(sample, "alt")),
                                            "vel_m_s" : float(get_value_from_msg(sample, "vel_m_s", default=0.00000)),
                                            "satellites_used" : int(get_value_from_msg(sample, "satellites_used", default=0)),
                                            "device_id" : int(get_value_from_msg(sample, "device_id").split()[0])
                                         },
                                         lambda resp: test_gps(resp, sample), resp)
                            
                        elif "sensor_baro" in sample:
                            sensor_retry("sensor_baro",
                                         lambda: {
                                            "temperature" : float(get_value_from_msg(sample, "temperature")),
                                            "pressure" : float(get_value_from_msg(sample, "pressure"))
                                         },
                                         test_baro, resp)

                        elif "battery_status" in sample:
                            sensor_retry("battery_status",
                                         lambda: {
                                            "voltage_filtered_v" : float(get_value_from_msg(sample, "voltage_filtered_v"))
                                         },
                                         test_battery, resp)

                        elif "input_rc" in sample:
                            sensor_retry("input_rc",
                                         lambda: {
                                            "rc_connection" : "True" if get_value_from_msg(sample, "rc_lost").strip() == "False" else "False",
                                            "rssi" : get_value_from_msg(sample, "rssi_dbm", default=0.0),
                                            "link_quality" : int(get_value_from_msg(sample, "link_quality", default=0.0))
                                         },
                                         test_rc, resp)

                    else:
                        resp["voxl-px4"]["error"] = True
                        resp["voxl-px4"]["result"] = False

            except:
                resp["voxl-px4"]["error"] = True
                resp["voxl-px4"]["result"] = False
        else:
            resp["voxl-px4"]["running"] = False
            resp["voxl-px4"]["result"] = False



def voxl_lepton_server_test(resp, assembly_sku, gimbal_type):
    result = True
    #
    # Platform checks
    #
    if "D0008" in assembly_sku:
        FLIR_I2C_BUS = "4"
    elif "D0013" in assembly_sku:
        FLIR_I2C_BUS = "0"
    elif "D0012" in assembly_sku or "D0014" in assembly_sku:
        if gimbal_type != 8:
            return
        else:
            FLIR_I2C_BUS = "4"
    else: 
        return

    FLIR_I2C_ADDR = "2a"
    # I2C FLIR Lepton on M0130 J8 - /dev/i2c0
    # i2cdetect -a -y -r 0
    p = subprocess.Popen(['i2cdetect', '-a', '-y', '-r', FLIR_I2C_BUS], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    i2c_detect_lines = output_str.split('\n')
    #print(i2c_detect_lines)

    #    0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
    #00: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    #10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    #20: -- -- -- -- -- -- -- -- -- -- 2a -- -- -- -- --
    #30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    #40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    #50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    #60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
    #70: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --

    i2c_detected = False
    resp["compute"]["i2c0"] = {}
    resp["compute"]["i2c0"]["result"] = False
    for line in i2c_detect_lines:
        if FLIR_I2C_ADDR in line:
            i2c_detected = True
            resp["compute"]["i2c0"]["result"] = True
            resp["compute"]["i2c0"]["probe"] = FLIR_I2C_ADDR
     
    if not i2c_detected:
        result = False

    #
    # SDK checks
    #
    if is_service_running("voxl-lepton-server"):
        resp["voxl-lepton-server"]["running"] = True

    # Check for intrinsics calibration file
    if path.exists(VOXL_FLIR_SERVER_INTRINSICS_FILE):
         resp["voxl-lepton-server"]["intrinsics"] = VOXL_FLIR_SERVER_INTRINSICS_FILE
    else:
         result = False

    # Check for MPA pipes
    VOXL_FLIR_SERVER_PIPE_COLOR="/run/mpa/lepton0_color/info"
    VOXL_FLIR_SERVER_RAW_COLOR="/run/mpa/lepton0_raw/info"
    if path.exists(VOXL_FLIR_SERVER_PIPE_COLOR):
        resp["voxl-lepton-server"]["mpaColor"] = VOXL_FLIR_SERVER_PIPE_COLOR
    else:
        result = False
    if path.exists(VOXL_FLIR_SERVER_RAW_COLOR):
        resp["voxl-lepton-server"]["mpaRaw"] = VOXL_FLIR_SERVER_RAW_COLOR
    else:
        result = False

    resp["voxl-lepton-server"]["result"] = result

def voxl_lepton_tracker_test(resp):
    svc = "voxl-lepton-tracker"
    result = True

    #
    # Platform checks
    #

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    VOXL_LEPTON_TRACKER_POS="/run/mpa/lepton0_16raw/info"
    if path.exists(VOXL_LEPTON_TRACKER_POS):
        resp["voxl-lepton-tracker"]["mpaPipe"] = VOXL_LEPTON_TRACKER_POS
    else:
        result = False

    resp[svc]["result"] = result


def voxl_qvio_server_test(resp):
    svc = "voxl-qvio-server"
    result = True

    #
    # Platform checks
    #

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    VOXL_QVIO_SERVER_PIPE="/run/mpa/qvio/info"
    if path.exists(VOXL_QVIO_SERVER_PIPE):
        resp[svc]["mpaPipe"] = VOXL_QVIO_SERVER_PIPE
    else:
        result = False

    resp[svc]["result"] = result

def voxl_open_vins_server_test(resp):
    svc = "voxl-open-vins-server"
    result = True

    #
    # Platform checks
    #

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    VOXL_OV_SERVER_PIPE = "/run/mpa/ov/info"
    VOXL_OV_EXTENDED_PIPE = "/run/mpa/ov_extended/info"
    VOXL_OV_OVERLAY_PIPE = "/run/mpa/ov_overlay/info"
    VOXL_OV_STATUS_PIPE = "/run/mpa/ov_status/info"

    ov_pipes = {
        "ov": VOXL_OV_SERVER_PIPE,
        "ov_extended": VOXL_OV_EXTENDED_PIPE,
        "ov_overlay": VOXL_OV_OVERLAY_PIPE,
        "ov_status": VOXL_OV_STATUS_PIPE
    }

    for key, pipe_path in ov_pipes.items():
        if path.exists(pipe_path):
            resp[svc][key] = pipe_path
        else:
            result = False

    resp[svc]["result"] = result

def voxl_vision_px4_test(resp):
    svc = "voxl-vision-px4"
    result = True

    #
    # Platform checks
    #

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    resp[svc]["result"] = result


def voxl_vision_hub_test(resp, exp_type):
    svc = "voxl-vision-hub"
    result = True

    vhubPipes = {
        "mpaPipeVoaPcOut": "/run/mpa/voa_pc_out/info",
        "mpaPipeFixed": "/run/mpa/vvhub_body_wrt_fixed/info",
        "mpaPipeLocal": "/run/mpa/vvhub_body_wrt_local/info",
        "mpaPipeFixedPoseInput": "/run/mpa/vvhub_fixed_pose_input"
    }

    if "D0008" in assembly_sku or "D0014" in assembly_sku or exp_type == 8:
        vhubPipes.pop("mpaPipeFixed", None)
        vhubPipes.pop("mpaPipeLocal", None)

    #
    # Platform checks
    #

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    for pipe_name, pipe_path in vhubPipes.items():
        resp[svc][pipe_name] = pipe_path
        result &= path.exists(pipe_path)

    resp[svc]["result"] = result


def voxl_feature_tracker_test(resp):
    svc = "voxl-feature-tracker"
    result = True

    #
    # Platform checks
    #

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    VOXL_FEATURE_TRACKER_TRACKED_FEATS="/run/mpa/tracked_feats/info"
    if path.exists(VOXL_FEATURE_TRACKER_TRACKED_FEATS):
        resp["voxl-feature-tracker"]["mpaPipe"] = VOXL_FEATURE_TRACKER_TRACKED_FEATS
    else:
        result = False

    resp[svc]["result"] = result


def voxl_mavlink_server_test(resp):
    svc = "voxl-mavlink-server"
    result = True

    #
    # Platform checks
    #

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    resp[svc]["result"] = result


# OUTDATED
# def voxl_flow_server_test(resp):
#     svc = "voxl-flow-server"
#     result = True

#     #
#     # Platform checks
#     #

#     #
#     # SDK checks
#     #
#     if is_service_running(svc):
#         resp[svc]["running"] = True
#     else:
#         result = False

#     VOXL_FLOW_SERVER_POS="/run/mpa/lepton0_16raw/info"
#     if path.exists(VOXL_FLOW_SERVER_POS):
#         resp["voxl-flow-server"]["mpaPipe"] = VOXL_FLOW_SERVER_POS
#     else:
#         result = False

#     resp[svc]["result"] = result


def voxl_uvc_server_test(resp, gimbal_type):
    # Check if boson from SKU
    if gimbal_type != 2:
        return
    result = True
    
    #
    # Platform checks
    #
    result = True

    # Check for boson enumeration
    lsusb_command = ['lsusb','-d','09cb:4007']
    try:
        lsusb = subprocess.check_output(lsusb_command, stderr=subprocess.DEVNULL, ).decode("utf-8")
        if "ID 09cb:4007" in lsusb:
            resp["voxl-uvc-server"]["device"] = "09cb:4007"
    except subprocess.CalledProcessError:
        pass

    #
    # SDK checks
    #
    if is_service_running("voxl-uvc-server"):
        resp["voxl-uvc-server"]["running"] = True
    else:
        result = False

    VOXL_UVC_SERVER_POS="/run/mpa/uvc/info"
    if path.exists(VOXL_UVC_SERVER_POS):
        resp["voxl-uvc-server"]["mpaPipe"] = VOXL_UVC_SERVER_POS
    else:
        result = False

    resp["voxl-uvc-server"]["result"] = result


def voxl_imu_server_test(resp):
    svc = "voxl-imu-server"
    result = True

    #
    # Platform checks
    #
    # None... internal accel and gyro

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    VOXL_IMU_SERVER_POS="/run/mpa/imu_apps/info"
    if path.exists(VOXL_IMU_SERVER_POS):
        resp[svc]["mpaPipe"] = VOXL_IMU_SERVER_POS
    else:
        result = False

    # Test using voxl-imu-server -t 
    imu_command = ['voxl-imu-server', '-t']
    imu_test = subprocess.check_output(imu_command).decode("utf-8")
    if "FAIL" in imu_test:
        resp[svc]["result"] = False

    imu_lines = imu_test.splitlines()
    if "detected"  == imu_lines[1].split()[0]:
        resp[svc]["device"] = imu_lines[1].split()[1]

    # Restart the service... voxl-imu-server -t stops the service after testing
    imu_command.pop(1)
    restart_command = ["systemctl", "restart"]
    restart_command += imu_command
    try:
        _ = subprocess.check_output(restart_command, timeout=15)
    except subprocess.TimeoutExpired:
        print("Command execution timed out.")

    resp[svc]["result"] = result

def voxl_px4_imu_server_test(resp):
    svc = "voxl-px4-imu-server"
    result = True

    #
    # Platform checks
    #
    # None... internal accel and gyro

    #
    # SDK checks
    #
    if is_service_running(svc):
        resp[svc]["running"] = True
    else:
        result = False

    VOXL_IMU_SERVER_POS="/run/mpa/imu_px4/info"
    if path.exists(VOXL_IMU_SERVER_POS):
        resp[svc]["mpaPipe"] = VOXL_IMU_SERVER_POS
    else:
        result = False

    resp[svc]["result"] = result


def voxl_modem_test(resp, modem_type):
    """
    param resp: full JSON data model of test target
    """
    if modem_type == 0:
        return
    result = False
    voxl_modem_command = ['ifconfig']

    output = subprocess.check_output(voxl_modem_command).decode('utf-8')
    if modem_type in {3, 10, 11, 27, 29}: # Microhard
        if "usb0" in output:
            resp["voxl-modem"]["enum"] = True
            interface = "usb0"
    elif modem_type in {18, 19, 23}: # Doodle
        if "eth0" in output:
            resp["voxl-modem"]["enum"] = True
            interface = "eth0"
    elif modem_type in {4, 7, 8, 12, 20, 21, 22, 24}: # LTE
        if "wwan0" in output:
            resp["voxl-modem"]["enum"] = True
            interface = "wwan0"
    elif modem_type in {13, 16, 17, 25, 26, 28}: # Alfa Networks / VTX
        if "wlan0" in output:
            resp["voxl-modem"]["enum"] = True
            interface = "wlan0"

    if resp["voxl-modem"]["enum"] == True:
        voxl_modem_command.append(interface)
        inet_output = subprocess.check_output(voxl_modem_command).decode('utf-8')
        inet_address = re.search(r'inet (\d+\.\d+\.\d+\.\d+)', inet_output)
        if inet_address:
            resp["voxl-modem"]["inet"] = inet_address.group(1)
            result = True
    else:
        result = False
    
    resp["voxl-modem"]["result"] = result

def voxl_rangefinder_test(resp):
    svc = "voxl-rangefinder-server"
    result = True

    if is_service_running(svc):
        resp[svc]["running"] = True

        pipe_path = "/run/mpa/rangefinders/info"
        if path.exists(pipe_path):
            output = subprocess.run("voxl-inspect-rangefinders -t".split(), stdout=subprocess.PIPE, stderr=subprocess.PIPE)
            stdout_str = output.stdout.decode('utf-8')

            if "TEST PASSED" in stdout_str:
                resp[svc]["mpaPipe"] = pipe_path
            else:
                result = False
    else:
        result = False
    
    resp[svc]["result"] = result

    


def voxl_calibration_test(resp):
    output = subprocess.run("voxl-check-calibration".split(), stdout = subprocess.DEVNULL)
    if not output.returncode:
        resp["calOk"] = True

#
# Factory Tests
#

def factory_test_d0004(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()

    voxl_px4_factory_test(resp, "D0004", enable_rc)
    voxl_camera_server_health_check(resp, "D0004", dmesg_lines)
    voxl_vision_hub_test(resp)
    voxl_qvio_server_test(resp)
    voxl_mavlink_server_test(resp)
    voxl_px4_imu_server_test(resp)
    voxl_calibration_test(resp)

    return overall_result


def factory_test_d0005(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()
    
    num_cams, gimbal_type, modem_type, camera_config, exp_type = parse_skus(resp["sku"])

    voxl_px4_factory_test(resp, "D0005", enable_rc)
    voxl_camera_server_health_check(resp, "D0005", dmesg_lines)
    voxl_vision_hub_test(resp, exp_type)
    voxl_qvio_server_test(resp)
    voxl_mavlink_server_test(resp)
    voxl_imu_server_test(resp)
    voxl_calibration_test(resp)

    return overall_result


def factory_test_d0006(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()

    num_cams, gimbal_type, modem_type, camera_config, exp_type = parse_skus(resp["sku"])

    voxl_px4_factory_test(resp, "D0006", enable_rc)
    voxl_camera_server_health_check(resp, "D0006", dmesg_lines)
    voxl_vision_hub_test(resp, exp_type)
    voxl_qvio_server_test(resp)
    voxl_mavlink_server_test(resp)
    voxl_imu_server_test(resp)
    voxl_calibration_test(resp)

    return overall_result


def factory_test_d0008(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()

    num_cams, gimbal_type, modem_type, camera_config, exp_type = parse_skus(resp["sku"])

    voxl_px4_factory_test(resp, "D0008", enable_rc)
    voxl_camera_server_health_check(resp, "D0008", dmesg_lines)
    voxl_lepton_server_test(resp, "D0008", gimbal_type)
    voxl_lepton_tracker_test(resp)
    voxl_vision_hub_test(resp, exp_type)
    voxl_open_vins_server_test(resp)
    voxl_mavlink_server_test(resp)
    voxl_uvc_server_test(resp, gimbal_type)
    voxl_imu_server_test(resp)
    voxl_modem_test(resp, modem_type)
    voxl_calibration_test(resp)

    return overall_result

def factory_test_d0012(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()
    num_cams, gimbal_type, modem_type, camera_config, exp_type = parse_skus(resp["sku"])

    voxl_px4_factory_test(resp, "D0012", enable_rc)
    voxl_camera_server_health_check(resp, "D0012", dmesg_lines)
    voxl_vision_hub_test(resp, exp_type)
    voxl_open_vins_server_test(resp)
    voxl_mavlink_server_test(resp)
    voxl_imu_server_test(resp)
    voxl_rangefinder_test(resp)
    voxl_lepton_server_test(resp, "D0012", gimbal_type)
    voxl_modem_test(resp, modem_type)
    voxl_calibration_test(resp)

    return overall_result

def factory_test_d0013(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()
    
    num_cams, gimbal_type, modem_type, camera_config, exp_type = parse_skus(resp["sku"])
    
    voxl_px4_factory_test(resp, "D0013", enable_rc)
    voxl_camera_server_health_check(resp, "D0013", dmesg_lines)
    voxl_vision_hub_test(resp, exp_type)
    voxl_open_vins_server_test(resp)
    voxl_mavlink_server_test(resp)
    voxl_open_vins_server_test(resp)
    voxl_imu_server_test(resp)
    voxl_lepton_server_test(resp, "D0013", gimbal_type)
    voxl_lepton_tracker_test(resp)
    voxl_calibration_test(resp)

    return overall_result

def factory_test_d0014(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()
    num_cams, gimbal_type, modem_type, camera_config, exp_type = parse_skus(resp["sku"])

    voxl_px4_factory_test(resp, "D0014", enable_rc)
    voxl_camera_server_health_check(resp, "D0014", dmesg_lines)
    voxl_vision_hub_test(resp, exp_type)
    voxl_mavlink_server_test(resp)
    voxl_imu_server_test(resp)
    voxl_rangefinder_test(resp)
    voxl_open_vins_server_test(resp)
    voxl_lepton_server_test(resp, "D0014", gimbal_type)
    voxl_modem_test(resp, modem_type)
    voxl_calibration_test(resp)

    return overall_result

def factory_test_d0015(resp, enable_rc=False):
    overall_result = True

    # get dmesg output
    p = subprocess.Popen(['dmesg'], stdout=subprocess.PIPE)
    output, err = p.communicate()
    output_str = output.decode('utf-8')
    dmesg_lines = output_str.split('\n')

    resp["uid"] = subprocess.check_output("cat /sys/devices/soc0/serial_number".split()).decode("utf-8").strip()
    num_cams, gimbal_type, modem_type, camera_config, exp_type = parse_skus(resp["sku"])

    voxl_px4_factory_test(resp, "D0015", enable_rc)
    voxl_camera_server_health_check(resp, "D0015", dmesg_lines)
    voxl_vision_hub_test(resp, exp_type)
    voxl_qvio_server_test(resp)
    voxl_mavlink_server_test(resp)
    voxl_imu_server_test(resp)
    # voxl_rangefinder_test(resp)
    # voxl_lepton_server_test(resp, "D0014", gimbal_type)
    # voxl_modem_test(resp, modem_type)
    voxl_calibration_test(resp)

    return overall_result


if __name__ == "__main__":
    log_path = "/var/log"
    max_log_bytes = 20000000000 # 20 GB
    args = parse_args()
    resp = {}
    resp["sku"] = "NA"
    resp["healthCheckResult"] = "false"
    resp["diskData"] = "false"  

    if args["help"]:
        print_usage()
        exit(0)
    
    if args["enable_rc"]:
        _px4_model["input_rc"] = {}
        _px4_model["input_rc"]["result"] = False
        _px4_model["input_rc"]["rc_connection"] = False
        _px4_model["input_rc"]["rssi_dbm"] = 0
        _px4_model["input_rc"]["link_quality"] = 0
 
    res, assembly_sku = read_file_line(SKU)

    sku_models = {
        "D0004": _d0004_model,
        "D0005": _d0005_model,
        "D0006": _d0006_model,
        "D0008": _d0008_model,
        "D0012": _d0012_model,
        "D0013": _d0013_model,
        "D0014": _d0014_model,
        "D0015": _d0015_model
    }

    if res and any(sku_type in assembly_sku for sku_type in sku_models):
        sku_type = next(sku for sku in sku_models if sku in assembly_sku)
        resp = sku_models[sku_type]
        resp["sku"] = assembly_sku

        if os.path.exists(log_path):
            data_disk = cleanup_logs(max_log_bytes) # clear /var/log when full  
            resp["diskData"] = f"{data_disk['size']}G/20G"

        factory_test_func = globals()[f"factory_test_{sku_type.lower()}"]
        if factory_test_func(resp, args.get("enable_rc", False)):
            resp["healthCheckResult"] = "true"

    if args["json"]:
        if print_readable(resp):
            resp["healthCheckResult"] = "true"
        else:
            resp["healthCheckResult"] = "false"
        resp_json = json.dumps(resp)
        print(resp_json)

    else:
        print_readable(resp)
