#!/usr/bin/env python3

import shepherd.config
import shepherd.module

import sys
import os
import time
import argparse

from gpiozero import OutputDevice, Device
from gpiozero.pins.pigpio import PiGPIOFactory

from shepherd.modules.betterservo import BetterServo

Device.pin_factory = PiGPIOFactory()


MOTHTRAP_LED_PIN = 6
MOTHTRAP_SERVO_PIN = 10
MOTHTRAP_SERVO_POWER_PIN = 9


class MothtrapConfDef(shepherd.config.ConfDefinition):
    def __init__(self):
        super().__init__()
        self.add_def('servo_open_pulse', shepherd.config.IntDef(default=1200))
        self.add_def('servo_closed_pulse', shepherd.config.IntDef(default=1800))
        self.add_def('servo_open_time', shepherd.config.IntDef(default=5))


class MothtrapModule(shepherd.module.SimpleModule):
    conf_def = MothtrapConfDef()

    def setup(self):

        print("Mothtrap config:")
        print(self.config)

        servo_max = self.config["servo_open_pulse"] / 1000000
        servo_min = self.config["servo_closed_pulse"] / 1000000

        if servo_min > servo_max:
            servo_min, servo_max = servo_max, servo_min

        self.door_servo = BetterServo(MOTHTRAP_SERVO_PIN, initial_value=None,
                                 active_high=False,
                                 min_pulse_width=servo_min,
                                 max_pulse_width=servo_max)

        self.door_servo_power = OutputDevice(MOTHTRAP_SERVO_POWER_PIN,
                                   active_high=True,
                                   initial_value=False)

        self.led_power = OutputDevice(MOTHTRAP_LED_PIN,
                                   active_high=True,
                                   initial_value=False)

    def setup_other_modules(self):
        self.modules.picam.hook_pre_cam.attach(self.led_on)
        self.modules.picam.hook_post_cam.attach(self.led_off)
        self.modules.picam.hook_post_cam.attach(self.run_servo)

    def led_on(self):
        self.led_power.on()

    def led_off(self):
        self.led_power.off()

    def run_servo(self):
        self.door_servo_power.on()
        time.sleep(0.5)
        self.door_servo.pulse_width = self.config["servo_open_pulse"] / 1000000
        time.sleep(self.config["servo_open_time"])
        self.door_servo.pulse_width = self.config["servo_closed_pulse"] / 1000000
        time.sleep(self.config["servo_open_time"])
        self.door_servo.detach()
        self.door_servo_power.off()


def main(argv):
    argparser = argparse.ArgumentParser(
      description='Module for mothtrap control functions. Run for testing')
    argparser.add_argument("configfile", nargs='?', metavar="configfile",
                           help="Path to configfile", default="conf.toml")
    argparser.add_argument("test_function", nargs='?', choices=['servo'],
                           help="test function to perform", default="servo")
    argparser.add_argument("-o", help="servo open position, in us", type=int, default=1200, dest="servo_open_pulse")
    argparser.add_argument("-c", help="servo closed position, in us", type=int, default=1800, dest="servo_closed_pulse")
    argparser.add_argument("-w", help="wait time, in seconds", type=int, default=5, dest="servo_open_time")

    args = argparser.parse_args()
    confman = shepherd.config.ConfigManager()

    srcdict = {"mothtrap": {"servo_open_pulse": args.servo_open_pulse,
                            "servo_closed_pulse":args.servo_closed_pulse,
                            "servo_open_time":args.servo_open_time}}

    if os.path.isfile(args.configfile):
        confman.load(args.configfile)
    else:
        confman.load(srcdict)

    mothtrap_mod = MothtrapModule(confman.get_config("mothtrap", MothtrapConfDef()),
                                  shepherd.module.Interface(None))
                                  
    mothtrap_mod.led_on()
    mothtrap_mod.run_servo()
    mothtrap_mod.led_off()


if __name__ == "__main__":
    main(sys.argv[1:])