#!/usr/bin/env python3

import asyncio
import sys
import argparse

from mavsdk import System

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

    parser.add_argument('-f', '--file', help='Extras.txt file with updated rates')
    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)

    asyncio.ensure_future(observe_shell(drone))

    print("Waiting for drone to connect...")
    async for state in drone.core.connection_state():
        if state.is_connected:
            print(f"-- Connected to drone!")
            break

    asyncio.get_event_loop().add_reader(sys.stdin, got_stdin_data, drone)
    print("nsh> ", end='', flush=True)


async def observe_shell(drone):
    async for output in drone.shell.receive():
        print(f"\n{output} ", end='', flush=True)


def got_stdin_data(drone):
    asyncio.ensure_future(send(drone, sys.stdin.readline()))


async def send(drone, command):
    await drone.shell.send(command)


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

    try:
        asyncio.get_event_loop().run_forever()
    except KeyboardInterrupt:
        pass
