#!/usr/bin/env python3

# The best way to start this server is from the parent folder using:
#   python3 -m stage.valve_server --quiet

cmd_desc = "Communicate real-time pneumatic commands received via OSC to the valve hardware."

import argparse
import time
import math
import logging
import queue
import threading

import numpy as np

# import the pythonosc package
from pythonosc import udp_client
from pythonosc import dispatcher
from pythonosc import osc_server

# import the ValveControl interface
from . import valves

# OSC port for valve commands
from .config import valve_UDP_port, valve_host_IP

# common logging functions
from .logconfig import add_logging_args, open_log_file, configure_logging

# initialize logging for this module
log = logging.getLogger('valve')

#================================================================
class EventLoop:
    def __init__(self, args):

        # configure event timing
        self.dt_ns = 500*1000*1000  # 2 Hz in nanoseconds per cycle

        # save flags
        self.verbose = args.verbose

        # Keep usage counts for logging.
        self.msg_count = 0

        # Open the Arduino serial port
        log.info("Opening Arduino serial port %s.", args.arduino)
        self.valves = valves.ValveControlClient(port=args.arduino, verbose=args.verbose, debug=args.debug)

        log.info("Waiting for Arduino wakeup.")
        self.valves.wait_for_wakeup()

        # Create the networking listener
        self.init_networking(args)

        return

    def close(self):
        # Issue a command to turn off the valves, then shut down the connections.
        log.info("Closing serial ports.")
        self.valves.send_stop()
        self.valves.close()

    #---------------------------------------------------------------
    def init_networking(self, args):
        # Initialize the OSC message dispatch system.
        self.dispatch = dispatcher.Dispatcher()
        self.dispatch.map("/move",     self.valves_move)
        self.dispatch.map("/empty",    self.valves_empty)

        self.dispatch.set_default_handler(self.unknown_message)

        # Start and run the server.
        unit_port = valve_UDP_port + args.unit
        self.server = osc_server.OSCUDPServer((args.ip, unit_port), self.dispatch)
        self.server_thread = threading.Thread(target=self.server.serve_forever)
        self.server_thread.daemon = True
        self.server_thread.start()
        log.info("started OSC server on port %s:%d", args.ip, unit_port)

    def unknown_message(self, msgaddr, *args):
        """Default handler for unrecognized OSC messages."""
        log.warning("Received unmapped OSC message %s: %s", msgaddr, args)

    def valves_move(self, msgaddr, *args):
        """Process /move messages received via OSC over UDP.  The first
        message value specifies a joint identifier, the second a PWM rate."""
        try:
            joint  = int(args[0])
            pwm    = int(args[1])
            self.valves.send_move(joint, pwm)
            self.msg_count += 1

        except:
            log.warning("error processing OSC move message: %s", args)

    def valves_empty(self, msgaddr, *args):
        """Process /empty messages received via OSC over UDP.  The message has no arguments."""
        try:
            self.valves.send_empty()
            self.msg_count += 1

        except:
            log.warning("error processing OSC move message: %s", args)

    #---------------------------------------------------------------
    # Event loop.  Most of the data flow occurs on the OSC callbacks as they
    # directly send messages to the Arduino.  The event loop
    # only processes Arduino output and periodically updates the log.
    def run(self):
        start_t = time.monotonic_ns()
        next_cycle_t = start_t + self.dt_ns
        event_cycles = 0

        while True:
            # wait for the next cycle timepoint, keeping the long
            # term rate stable even if the short term timing jitters
            now_ns = time.monotonic_ns()
            delay = max(next_cycle_t - now_ns, 0)
            if (delay > 0):
                time.sleep(delay * 1e-9)
            next_cycle_t += self.dt_ns
            now_ns = time.monotonic_ns()
            now_seconds = (now_ns - start_t)*1e-9

            # Process status messages from the Arduino.
            self.valves.poll_status()

            # periodically log status
            if (event_cycles % (2*60*5)) == 0:
                log.debug("event cycle %d: msgs: %d", event_cycles, self.msg_count)
            event_cycles += 1


#================================================================
# The following section is run when this is loaded as a script.
if __name__ == "__main__":

    # Initialize the command parser.
    parser = argparse.ArgumentParser(description = cmd_desc)
    parser.add_argument( '--ip', default=valve_host_IP,  help="Network interface address (default: %(default)s).")
    parser.add_argument( '--arduino', default='/dev/ttyACM0', help='Arduino serial port device (default is %(default)s.)')
    parser.add_argument( '--unit', default=0, type=int, help='Unit number (default is %(default)s.)')
    add_logging_args(parser)

    # Parse the command line, returning a Namespace.
    args = parser.parse_args()

    # set up logging
    open_log_file('logs/valve-server-%d.log' % (args.unit))
    log.info("Starting valve-server.py as unit %d", args.unit)

    # Modify logging settings as per common arguments.
    configure_logging(args)

    # Create the event loop.
    event_loop = EventLoop(args)

    # Begin the performance.  This may be safely interrupted by the user pressing Control-C.
    try:
        event_loop.run()

    except KeyboardInterrupt:
        log.info("User interrupted operation.")
        print("User interrupt, shutting down.")
        event_loop.close()

    except Exception as e:
        log.error("unable to continue: %s: %s", type(e), e)
        print("unable to continue: ", type(e), e)