#!/usr/bin/env python3

import asyncio
import argparse
from tqdm import tqdm

from mavsdk import System

def arg_parser():
    parser = argparse.ArgumentParser(
        prog='calibrate.py',
        epilog="See '<command> --help' for more detail")

    parser.add_argument('-f', '--param_file', help='Param file to be uploaded with .params format')
    parser.add_argument('--port', type=str, default="/dev/pixhawk")
    parser.add_argument('--baudrate', type=int, default=2000000)

    args = parser.parse_args()

    return args

async def main(args):
    drone = System()
    serial = "serial://" + str(args.port) + ":" + str(args.baudrate) 
    await drone.connect(system_address=serial)
    print("Connected to a device")

    # parse out parameters
    param_plugin = drone.param
    params = await param_plugin.get_all_params()
    float_params = params.float_params
    int_params = params.int_params
    custom_params = params.custom_params
    int_param_names = [p.name for p in int_params]
    float_param_names = [p.name for p in float_params]
    custom_param_names = [p.name for p in custom_params]

    # # Get the list of parameters
    # for param in params.int_params:
    #     print(f"{param.name}: {param.value}")

    # for param in params.float_params:
    #     print(f"{param.name}: {param.value}")


    async for is_in_air in drone.telemetry.in_air():
        if is_in_air:
            print("Waiting until vehicle is landed...")
        else:
            break

    with open(args.param_file, "r") as param_file:
        print("Uploading Parameters... Please do not arm the vehicle!")
        for line in tqdm(param_file, unit='lines'):
            if line.startswith("#"):
                continue

            columns = line.strip().split(",")
            name = columns[0]
            value = columns[1]
            if name in int_param_names:
                await drone.param.set_param_int(name, int(value))
            elif name in float_param_names:
                await drone.param.set_param_float(name, float(value))
            elif name in custom_param_names:
                await drone.param.set_param_custom(name, value)

    print("Params uploaded, rebooting!")
    await drone.action.reboot()


if __name__ == "__main__":
    asyncio.run(main(args = arg_parser())) 
