#!/usr/bin/python3
# coding: UTF-8

import time
import sys
import usb

# commands
DOWN = 0x01
UP = 0x02
LEFT = 0x04
RIGHT = 0x08
FIRE = 0x10
STOP = 0x20

# measured geometry
ANGLE_H_MAX = 139.0  # maximum ± horizontal degrees, 0 = center
H_DEG_PER_S = 50.7  # degrees per second of horizontal rotation
# vertical angle range, 0 = parallel to ground
ANGLE_V_MAX = 25.5
ANGLE_V_MIN = -5.5
# launcher needs 0.76 s from V_MIN to V_MAX (different speed downwards!)
V_DEG_PER_S = 40.8

# horizontal launcher at 0.8m height fires between 3.6 m and 4.2 m
FIRE_SPEED = 0.965  # this is quite imprecise, between 0.9 and 1.03 m/s

class LauncherDriver:
    def __init__(self):
        '''Initialize USB communication with Launcher'''

        # see https://github.com/walac/pyusb/blob/master/docs/tutorial.rst
        self.dev = usb.core.find(idVendor=0x2123, idProduct=0x1010)
        if not self.dev:
            sys.stderr.write('ERROR: Launcher not found\n')
            sys.exit(1)
        if self.dev.is_kernel_driver_active(0):
            print('detaching kernel driver')
            self.dev.detach_kernel_driver(0)
        self.dev.set_configuration()

    def cmd(self, cmd):
        '''Send a command (e. g. UP) to the launcher'''

        self.dev.ctrl_transfer(0x21, 0x09, 0, 0,
                               [0x02, cmd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])

    def set_h(self, angle):
        '''Set horizontal direction to given angle.

        Positive numbers are left of center, negative numbers are right of
        center.
        '''
        if angle >= 0:
            assert angle <= ANGLE_H_MAX

            # move all the way to the left for a defined location
            self.cmd(LEFT)
            time.sleep(2 * ANGLE_H_MAX / H_DEG_PER_S + 0.3)
            self.cmd(RIGHT)
            time.sleep((ANGLE_H_MAX - angle) / H_DEG_PER_S)
            self.cmd(STOP)
        else:
            assert angle >= -ANGLE_H_MAX

            # move all the way to the right for a defined location
            self.cmd(RIGHT)
            time.sleep(2 * ANGLE_H_MAX / H_DEG_PER_S + 0.3)
            self.cmd(LEFT)
            time.sleep((ANGLE_H_MAX + angle) / H_DEG_PER_S)
            self.cmd(STOP)

    def set_v(self, angle):
        '''Set vertical direction to given angle.

        Positive numbers are facing above horizontal plane, negative numbers
        face below horizontal plane.
        '''
        assert angle >= ANGLE_V_MIN
        assert angle <= ANGLE_V_MAX

        # move all the way down for a defined location
        self.cmd(DOWN)
        time.sleep((ANGLE_V_MAX - ANGLE_V_MIN) / V_DEG_PER_S + 0.1)
        self.cmd(UP)
        time.sleep((angle - ANGLE_V_MIN) / V_DEG_PER_S)
        self.cmd(STOP)


l = LauncherDriver()
# time-based relative movement:
# l.cmd(RIGHT + UP)
# time.sleep(0.3)
# l.cmd(RIGHT)
# time.sleep(1.3)
# l.cmd(STOP)

# absolute angle movement:
l.set_h(10)
l.set_v(10)

l.cmd(FIRE)