#!/bin/python3
import os
import re
import argparse
import fcntl
import csv
from smbus2 import SMBus
from subprocess import Popen, PIPE, CalledProcessError, call
from time import sleep

# these values are the duty cycle MIN and MAX
# the chip can take a max 0xFFF and min 0
# these values were found just by experimentation
SERVO_MIN = 100
SERVO_MAX = 500
SERVO_STEP = 5

# I2C address stuff
MODE1 = 0x00
MODE2 = 0x01
LED0_ON_L = 0x06
LED0_ON_H = 0x07
LED0_OFF_L = 0x08
LED0_OFF_H = 0x09
PRE_SCALE = 0xFE


def __i2c_write(addr: int, value: int):
    with SMBus(3) as bus:
        bus.write_byte_data(0x40, addr & 0xFF, value & 0xFF)


def __i2c_read(addr: int) -> int:
    with SMBus(3) as bus:
        data = bus.read_byte_data(0x40, addr & 0xFF)
    return data


def __pca9685_init():
    print("Init pca9685")
    # check if we are already enabled
    if (__i2c_read(MODE1) & 0x10) == 0x00:
        return

    # set frequency, 50Hz I think
    __i2c_write(PRE_SCALE, 0x79)

    sleep(1)

    # turn on servos
    __i2c_write(MODE1, 0x81)
    sleep(1)


def __pca9685_deinit():
    print("Deinit pca9685")
    __i2c_write(MODE1, 0x11)


def __servo_on(pwm_channel: int):
    print("Turn on " + str(pwm_channel))
    addrl = LED0_ON_L + (pwm_channel * 4)
    addrh = LED0_ON_H + (pwm_channel * 4)
    __i2c_write(addrl, 0x00)
    __i2c_write(addrh, 0x00)

    addrl = LED0_OFF_L + (pwm_channel * 4)
    addrh = LED0_OFF_H + (pwm_channel * 4)
    __i2c_write(addrl, 0x00)
    # leave the servo off until we write our first valid value
    __i2c_write(addrh, 0x10)


def __servo_off(pwm_channel: int):
    print("Turn off " + str(pwm_channel))
    addr = LED0_ON_H + (pwm_channel * 4)
    __i2c_write(addr, 0x00)
    addr = LED0_OFF_H + (pwm_channel * 4)
    __i2c_write(addr, 0x10)


def __servo_move(pwm_channel: int, value: int):
    print("Moving " + str(pwm_channel) + " " + str(value))
    # we only set the off value the on value is set during init
    addrl = LED0_OFF_L + (pwm_channel * 4)
    addrh = LED0_OFF_H + (pwm_channel * 4)
    __i2c_write(addrl, value & 0xFF)
    __i2c_write(addrh, (value >> 8) & 0x0F)
    # doesn't seem to work if I don't do a read here.
    __i2c_read(addrh)


def __load_config(config):
    print("Loading config: " + config)
    with open(config, "r") as fp:
        reader = csv.DictReader(fp, delimiter=",")
        data_read = list(reader)
    return data_read


def __servo_calibrate(pwm_channel: int, position: int, direction: str, serial=""):
    if serial != "":
        wait_for_device = ["adb", "-s", serial, "wait-for-device"]
        monitor_event = ["adb", "-s", serial, "shell", "getevent", "/dev/input/event0"]
    else:
        # use device on usb, return error if more than one device
        wait_for_device = ["adb", "-d", "wait-for-device"]
        monitor_event = ["adb", "-d", "shell", "getevent", "/dev/input/event0"]

    print("Waiting for device " + serial)
    call(wait_for_device)

    try:
        __pca9685_init()
        __servo_on(pwm_channel)
        __servo_move(pwm_channel, position)

        with Popen(
            monitor_event, stdout=PIPE, stderr=PIPE, bufsize=1, universal_newlines=True
        ) as p:
            duty = position
            chstdout = p.stdout
            fd = chstdout.fileno()
            fl = fcntl.fcntl(fd, fcntl.F_GETFL)
            fcntl.fcntl(fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)
            while True:
                if direction == "CCW":
                    duty += SERVO_STEP
                else:
                    duty -= SERVO_STEP
                print("Test")
                __servo_move(pwm_channel, duty)
                sleep(1)

                # just check if any output is sent from getevent command
                if p.stdout.readline():
                    print("Button Pressed at " + str(duty))
                    break
            p.kill()
    finally:
        __servo_move(pwm_channel, position)
        sleep(1)
        __servo_off(pwm_channel)
        __pca9685_deinit()

    return duty


def servo_calibrate(args):
    servo = __servo_calibrate(
        args.pwm_channel, args.duty_cycle, args.direction, args.serial
    )
    print(servo)


def servo_move(args):
    __servo_move(args.pwm_channel, args.duty_cycle)


def servo_init(args):
    __pca9685_init()
    __servo_on(args.pwm_channel)


def servo_deinit(args):
    __servo_off(args.pwm_channel)
    __pca9685_deinit()


def mib_init(args):
    servos = __load_config(args.config_file)
    __pca9685_init()
    for servo in servos:
        if args.name != None and servo["NAME"] not in args.name:
            continue
        __servo_on(int(servo["PWM_CHANNEL"]))


def mib_deinit(args):
    servos = __load_config(args.config_file)
    for servo in servos:
        if args.name != None and servo["NAME"] not in args.name:
            continue
        __servo_off(int(servo["PWM_CHANNEL"]))
    __pca9685_deinit()


def mib_press(args):
    servos = __load_config(args.config_file)
    for servo in servos:
        if args.name != None and servo["NAME"] not in args.name:
            continue
        __servo_move(int(servo["PWM_CHANNEL"]), int(servo["PRESS"]))


def mib_release(args):
    servos = __load_config(args.config_file)
    for servo in servos:
        if args.name != None and servo["NAME"] not in args.name:
            continue
        __servo_move(int(servo["PWM_CHANNEL"]), int(servo["RELEASE"]))


def mib_calibrate(args):
    servos = __load_config(args.config_file)
    for servo in servos:
        if args.name != None and servo["NAME"] not in args.name:
            continue
        servo["PRESS"] = __servo_calibrate(
            int(servo["PWM_CHANNEL"]),
            int(servo["RELEASE"]),
            servo["DIRECTION"],
            args.serial,
        )

    print(servos)
    if args.output != "":
        with open(args.output, "w") as f:
            w = csv.DictWriter(f, servos[0])
            w.writeheader()
            w.writerows(servos)

    # arg parse often double prints this aditional help so have removed it.
    # will leave it here if I work out how to get it working with the
    # subcommands
    # '''\n''' +
    #     '''This program allows you to configure and run servo commands. \n''' +
    #     '''The format of the config files is plain csv with the following Columns: \n''' +
    #     '''    NAME           the name of the button, can be anything \n''' +
    #     '''    PWM_CHANNEL    the number of the pwm channel to use \n''' +
    #     '''    PRESS          the duty cycle value coresponding to a pressed button \n''' +
    #     '''    RELEASE        the duty cycle value for when the button is not pressed \n''' +
    #     '''    DIRECTION      either CW or CCW, used for calibrating the servos \n''' +
    #     ''' \n'''


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    subparsers = parser.add_subparsers(required=True)

    parse_servo = subparsers.add_parser(
        "servo", help="Control an individual servo using command line options"
    )
    parse_servo.add_argument(
        "pwm_channel", type=int, help="(int) pwm channel on the chip e.g. 8"
    )

    sp_servo = parse_servo.add_subparsers(required=True)

    parse_cal_servo = sp_servo.add_parser(
        "calibrate", help="Auto calibrate a servos PRESS value"
    )
    parse_cal_servo.add_argument(
        "-c",
        "--duty_cycle",
        type=int,
        default=300,
        help="the duty cycle for a known good position not touching the button default is 300",
    )
    parse_cal_servo.add_argument(
        "-d",
        "--direction",
        type=str,
        default="CW",
        choices=["CW", "CCW"],
        help="CCW or CW - make sure this is correct!",
    )
    parse_cal_servo.add_argument(
        "-s",
        "--serial",
        type=str,
        default="",
        help="the serial for the android device to pass to adb",
    )
    parse_cal_servo.set_defaults(func=servo_calibrate)

    parse_move_servo = sp_servo.add_parser(
        "move", help="Move the servo to a given value"
    )
    parse_move_servo.add_argument(
        "duty_cycle",
        type=int,
        default=300,
        help="the duty cycle for a known good position not touching the button default is 300",
    )
    parse_move_servo.set_defaults(func=servo_move)

    parse_init_servo = sp_servo.add_parser("init", help="Initalise the servo")
    parse_init_servo.set_defaults(func=servo_init)

    parse_deinit_servo = sp_servo.add_parser("deinit", help="deinitalise the servo")
    parse_deinit_servo.set_defaults(func=servo_deinit)

    parse_mib = subparsers.add_parser(
        "mib",
        help="Control servos using the mib_config csv file, by default actions are performed on all servos",
    )
    parse_mib.add_argument("config_file", type=str, help="The config file for the mib")
    parse_mib.add_argument(
        "-n",
        "--name",
        type=str,
        help="The name of the servo to control e.g. power,vol_up,vol_down",
        action="append",
    )

    sp_mib = parse_mib.add_subparsers(required=True)

    parse_cal_mib = sp_mib.add_parser(
        "calibrate",
        help="Auto calibrate the servos PRESS value when given a valid config file,need a valid RELEASE value to start",
    )
    parse_cal_mib.add_argument(
        "-s",
        "--serial",
        type=str,
        default="",
        help="the serial for the android device to pass to adb",
    )
    parse_cal_mib.add_argument(
        "-o", "--output", type=str, default="", help="Output file for the config"
    )
    parse_cal_mib.set_defaults(func=mib_calibrate)

    parse_init_mib = sp_mib.add_parser("init", help="Initalise the servos")
    parse_init_mib.set_defaults(func=mib_init)

    parse_deinit_mib = sp_mib.add_parser("deinit", help="Deinitalise the servos")
    parse_deinit_mib.set_defaults(func=mib_deinit)

    parse_press_mib = sp_mib.add_parser("press", help="Press the buttons")
    parse_press_mib.set_defaults(func=mib_press)

    parse_release_mib = sp_mib.add_parser("release", help="release the buttons")
    parse_release_mib.set_defaults(func=mib_release)

    args = parser.parse_args()
    args.func(args)