#!/usr/bin/python3

import argparse
import hid
import struct
import sys

parser = argparse.ArgumentParser(prog="afcs_set", description="Send USB reports to control AFCS HID device")
parser.add_argument('-d', '--hdg', type=int, default=-1)
parser.add_argument('-l', '--leds', type=int, default=0)
parser.add_argument('-a', '--alt', type=int, default=0xffff)
parser.add_argument('-v', '--vs', type=int, default=-1)
parser.add_argument('-f', '--flc', type=int, default=-1)
parser.add_argument('-m', '--mach', action="store_true")
parser.add_argument('-p', '--pitch', action="store_true")
parser.add_argument('-r', '--reboot', action="store_true")

args = parser.parse_args()

afcs = hid.Device(vid=0x1209, pid=0x0001)

if args.reboot:
    afcs.write(b'\x02\x02' + 5*b'\x00\x00')
    sys.exit(0)

def build_update_report(flags, hdg=0, leds=0, alt=0, vs=0, as_=0):
    afcs.write(struct.pack('<BBhhHhh', 0x2, flags & 0xf0, hdg, leds, alt, vs, as_))

flags = 0
if args.mach:
    flags = flags | 0x10
if args.pitch:
    flags = flags | 0x20
    
build_update_report(flags, args.hdg, args.leds, args.alt, args.vs, args.flc)