#!/usr/bin/python3

################################################################################
# Copyright 2023 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 argparse
import json
import subprocess
from time import sleep

# Parse script arguments
def parse_args():
    description = "portal-parser -s voxl-px4 -c px4-listener -a sensor_gyro"
    parser = argparse.ArgumentParser(description=description)
    parser.add_argument('-s', '--service', help='service to use',              required=True, type=str, default="")
    parser.add_argument('-c', '--command', help='command to use',              required=True, type=str, default="")
    parser.add_argument('-a', '--arg', help='arg to use',                   required=False, type=str, default="")
    args = vars(parser.parse_args())
    return args

def get_value_from_msg(msg, variable, status=False):
    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 0

if __name__ == "__main__":

    args = parse_args()
    service = args["service"]
    command = args["command"]
    arg = args["arg"]

    resp = {}
    resp["service"] = service
    resp["command"] = command
    resp["arg"] = arg
    resp["success"] = False

    if "voxl-px4" in service:
        if "listener" in command:
            listener_command = ['px4-listener', '--instance', '0', '-n', '1']
            listener_command.insert(3, arg)
            try:
                sample = subprocess.check_output(listener_command).decode("utf-8")
                if "never published" in sample:
                    resp["result"] = "Topic never published"
                    resp["success"] = False

                elif "sensor_accel" in arg:
                    accel = {
                        "accel_x" : float(get_value_from_msg(sample, "x")),
                        "accel_y" : float(get_value_from_msg(sample, "y")),
                        "accel_z" : float(get_value_from_msg(sample, "z"))
                    }
                    resp["result"] = accel
                    resp["success"] = True

                elif "sensor_gyro" in arg:
                    gyro = {
                        "gyro_x" : float(get_value_from_msg(sample, "x")),
                        "gyro_y" : float(get_value_from_msg(sample, "y")),
                        "gyro_z" : float(get_value_from_msg(sample, "z"))
                    }
                    resp["result"] = gyro
                    resp["success"] = True

                elif "sensor_baro" in arg:
                    baro = {
                        "temperature" : float(get_value_from_msg(sample, "temperature")),
                        "pressure" : float(get_value_from_msg(sample, "pressure"))
                    }
                    resp["result"] = baro
                    resp["success"] = True

                elif "sensor_mag" in arg:
                    mag = {
                        "xmag" : float(get_value_from_msg(sample, "x")),
                        "ymag" : float(get_value_from_msg(sample, "y")),
                        "zmag" : float(get_value_from_msg(sample, "z"))
                    }
                    resp["result"] = mag
                    resp["success"] = True

                elif "sensor_gps" in arg:
                    gps = {
                        "lon" : float(get_value_from_msg(sample, "lon")),
                        "lat" : float(get_value_from_msg(sample, "lat")),
                        "alt" : float(get_value_from_msg(sample, "alt")),
                        "vel" : float(get_value_from_msg(sample, "vel_m_s")),
                        "sats" : int(get_value_from_msg(sample, "satellites_used"))
                    }
                    resp["result"] = gps
                    resp["success"] = True

                elif "battery_status" in arg:
                    battery = {
                        "voltage" : float(get_value_from_msg(sample, "voltage_filtered_v")),
                    }
                    resp["result"] = battery
                    resp["success"] = True

                elif "input_rc" in arg:
                    input_rc = {
                        "rc_connection" : True if "False" in get_value_from_msg(sample, "rc_lost") else False,
                        "rssi" : get_value_from_msg(sample, "rssi_dbm"),
                        "link_quality" : int(get_value_from_msg(sample, "link_quality"))
                    }
                    resp["result"] = input_rc
                    resp["success"] = True
                
            except:
                resp["result"] = "Topic did not match any known topics or voxl-px4 is not running"
                resp["success"] = False

        elif "status" in command:
            status_command = ['voxl-inspect-services']
            sample = subprocess.check_output(status_command, shell=True).decode("utf-8")
            if "voxl-px4" in sample:
                status = get_value_from_msg(sample, "voxl-px4", True)
                voxl_px4_status = {
                    "enabled" : status[1][6:-5].strip(" "),
                    "running" : status[2][6:-5].strip(" ")
                }
                # print(f"Result: {voxl_px4_status}")
                resp["result"] = voxl_px4_status
                resp["success"] = True

        elif "commander" in command:
            stop_commander = "px4-commander stop"
            start_commander = "px4-commander start"
            mode_commander = "px4-commander mode manual"
            status_commander = "px4-commander status"
            arm_command = "px4-commander arm"
            disarm_command = "px4-commander disarm -f"

            # Disarm drone
            if arg == "disarm":
                # Disarm drone command 
                subprocess.run(disarm_command.split())
                
                # Check commander status to verify drone disarmed 
                sample = subprocess.check_output(status_commander.split()).decode("utf-8")
                status = get_value_from_msg(sample, "Arm state")
                if status is not 0 and "Standby" in status: 
                    resp["result"] =  "Disarmed"
                    resp["success"] = True 
                else:
                    resp["result"] =  f"Failed to disarm. Commander status: {status}"
                    
            # Arm drone
            elif arg == "arm":
                # Restart px4-commander and put into manual flight mode
                subprocess.run(stop_commander.split(), stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
                sleep(1)
                subprocess.run(start_commander.split(), stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
                sleep(1)
                subprocess.run(mode_commander.split(), stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
                sleep(1)

                sample = subprocess.run(status_commander.split(), stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
                # Check that px4-commander started 
                if "INFO  [commander] not running" not in sample.stdout.decode("utf-8"):
                    # Arm 
                    subprocess.run(arm_command.split(), stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)

                    # Check status
                    sample = subprocess.check_output(status_commander.split()).decode("utf-8")
                    status = get_value_from_msg(sample, "Arm state")
                    if status is not 0 and "Armed" in status: 
                        resp["result"] =  "Armed"
                        resp["success"] = True 
                    else:
                        resp["result"] =  f"Failed to arm. Commander Status: {status}"

            else:
                resp["result"] = f"Px4-commander failed to start or invalid arg: {arg}"

        elif "param" in command:
            px4_params_command = ['voxl-configure-px4-params','-nqp']
            if "D0008" == arg:
                px4_params_command.append("MRB-D0008")
                resp["file"] = "/usr/share/modalai/px4_params/v1.14/platforms/FPV_RevB.params"
            elif "D0006" == arg:
                px4_params_command.append("MRB-D0006")
                resp["file"] = "/usr/share/modalai/px4_params/v1.14/platforms/Sentinel_V1.params"
            elif "D0005" == arg:
                px4_params_command.append("MRB-D0005")
                resp["file"] = "/usr/share/modalai/px4_params/v1.14/platforms/Starling_V2.params"
            elif "D0004" == arg:
                px4_params_command.append("MRB-D0006")  # RB5 can use sentinel params until a set for SDK 1.0 is made
                resp["file"] = "/usr/share/modalai/px4_params/v1.14/platforms/Sentinel_V1.params"
            else:
                resp["success"] = False 
                resp["result"] = "Unknown SKU"
                resp["failures"] = f"Unrecognized SKU: {arg}"
                resp["file"] = ""
                resp_json = json.dumps(resp)
                print(resp_json)

            output = subprocess.run(px4_params_command, stdout=subprocess.PIPE)
            rc = output.returncode
            stdout = output.stdout
            if rc:
                resp["success"] = False 
                failures = ""
                for line in stdout.split("\n"):
                    if "FAILED" in line:
                        vals = line.split()
                        failures += vals[1] + ", "
                resp["failures"] = failures[:-2]
            else:
                resp["success"] = True
                resp["failures"] = False
        else:
            resp["result"] = "Unknown command"

        resp_json = json.dumps(resp)

    elif "voxl-camera-server" in service:
        if "status" in command:
            status_command = ['voxl-inspect-services']
            sample = subprocess.check_output(status_command, shell=True).decode("utf-8")
            if "voxl-camera-server" in sample:
                status = get_value_from_msg(sample, "voxl-camera-server", True)
                voxl_camera_server_status = {
                    "enabled" : status[1][6:-5].strip(" "),
                    "running" : status[2][6:-5].strip(" ")
                }
                # print(f"Result: {voxl_camera_server_status}")
                resp["result"] = voxl_camera_server_status
                resp["success"] = True
                
        else:
            resp["result"] = "Unknown command"

        resp_json = json.dumps(resp)
        
    elif "voxl-uvc-server" in service:
        if "status" in command:
            status_command = ['voxl-inspect-services']
            sample = subprocess.check_output(status_command, shell=True).decode("utf-8")
            if "voxl-uvc-server" in sample:
                status = get_value_from_msg(sample, "voxl-uvc-server", True)
                voxl_uvc_server_status = {
                    "enabled" : status[1][6:-5].strip(" "),
                    "running" : status[2][6:-5].strip(" ")
                }
                # print(f"Result: {voxl_uvc_server_status}")
                resp["result"] = voxl_uvc_server_status
                resp["success"] = True
            
        else:
            resp["result"] = "Unknown command"

        resp_json = json.dumps(resp)
        
    elif "voxl-lepton-server" in service:
        if "status" in command:
            status_command = ['voxl-inspect-services']
            sample = subprocess.check_output(status_command, shell=True).decode("utf-8")
            if "voxl-lepton-server" in sample:
                status = get_value_from_msg(sample, "voxl-lepton-server", True)
                voxl_lepton_server_status = {
                    "enabled" : status[1][6:-5].strip(" "),
                    "running" : status[2][6:-5].strip(" ")
                }
                resp["result"] = voxl_lepton_server_status
                resp["success"] = True

        else:
            resp["result"] = "Unknown command"

        resp_json = json.dumps(resp)

    else:
        resp["result"] = "Error - service not found"
        resp_json = json.dumps(resp)

    print(resp_json)
