#!/usr/bin/env python

# SPDX-License-Identifier: BSD-3-Clause
# SPDX-FileCopyrightText: Czech Technical University in Prague

"""
ROS node that reads configuration of I/O devices and the runs them.

Parameters:
- `~virtual_pins` (dict): The configuration dictionary specifying virtual pins.
- `~electronic_io_devices` (dict): The config dictionary specifying the devices.
- `~poll_rate` (float, optional): Rate at which all pins will be read and the device will produce output.
"""

import sys

import rospy

from electronic_io import InputDevice, OutputDevice, IOBoardClient, load_devices, load_virtual_pins
from electronic_io_msgs.msg import Readings
from electronic_io_msgs.srv import ReadRequest


class Devices:
    def __init__(self):
        io_topic = "io_board"

        self.io_board = IOBoardClient(io_topic)

        virtual_pins_conf = rospy.get_param("~virtual_pins", {})
        if not isinstance(virtual_pins_conf, dict):
            raise AttributeError("Invalid configuration of electronic_io/devices node. "
                                 "Parameter ~virtual_pins has to be a dictionary.")

        load_virtual_pins(virtual_pins_conf, self.io_board)

        devices_conf = rospy.get_param("~electronic_io_devices", None)
        if not isinstance(devices_conf, dict):
            raise AttributeError("Invalid configuration of electronic_io/devices node. "
                                 "Parameter ~electronic_io_devices has to be a dictionary.")

        self.devices = load_devices(devices_conf, self.io_board)

        if len(devices_conf) > 0 and len(self.devices) == 0:
            rospy.logfatal("Could not load any device, exiting.")
            sys.exit(1)

        self.io_sub = rospy.Subscriber(io_topic, Readings, self.on_io_readings, queue_size=10)

    def on_io_readings(self, readings):  # type: (Readings) -> None
        for device in self.devices.values():
            try:
                if isinstance(device, InputDevice):
                    device.update(readings)
                elif isinstance(device, OutputDevice) and device.has_readback():
                    device.get_readback_device().update(readings)
            except Exception as e:
                rospy.logerr_throttle(1.0, "Error updating " + device.get_name() + ": " + str(e))

    def run(self):
        poll_rate = rospy.get_param("~poll_rate", None)
        rate = rospy.Rate(float(poll_rate if poll_rate is not None else 1.0))
        while not rospy.is_shutdown():
            if poll_rate is not None:
                read_req = ReadRequest()
                for device in self.devices.values():
                    if isinstance(device, InputDevice):
                        device.add_read_request(read_req)
                    elif isinstance(device, OutputDevice) and device.has_readback():
                        device.get_readback_device().add_read_request(read_req)
                readings = self.io_board.read(read_req)
                self.on_io_readings(readings)
            try:
                rate.sleep()
            except rospy.ROSInterruptException:
                pass


if __name__ == '__main__':
    rospy.init_node("electronic_io_devices")
    devices = Devices()
    devices.run()
