#!/usr/bin/env python3
# This software is distributed under the terms of the MIT License.
# Copyright (c) 2025 Dmitry Ponomarev.
# Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
"""
DroneCAN node
"""

# Pylint notes:
# - dronecan exposes members dynamically (generated DSDL), which confuses static analysis.
# pylint: disable=no-member


import re
import sys
import time
import logging
import argparse
import importlib
from typing import Any, Callable, Optional, Union

from tsugite.backend.backend import BaseBackend

logger = logging.getLogger("dronecan_backend")

try:
    import serial
except ModuleNotFoundError:
    logger.critical("DroneCAN required: pip install pyserial")
    sys.exit(1)

try:
    import dronecan
except ModuleNotFoundError:
    logger.critical("DroneCAN required: pip install pydronecan setuptools==80.9.0")
    sys.exit(1)


NodeId = Optional[int]
ParamValue = Optional[Union[bool, int, float, str]]
RAW_COMMAND_SCALE = 81.91

class DronecanBackend(BaseBackend):
    def __init__(self, iface: str = "slcan:/dev/ttyACM0", node_id: NodeId = 100) -> None:
        self.node = dronecan.make_node(
            iface, node_id=node_id, bitrate=1_000_000, baudrate=1_000_000
        )
        logger.info("Started DroneCAN node id=%s on %s", node_id, iface)
        self.periodic_pubs = {}

        self._param_value: ParamValue = None
        self._info = {}

    def subscribe(self, topic: str, callback: Callable, node_id: NodeId = None) -> None:
        data_type = DronecanBackend._topic_to_data_type(topic)
        if not data_type:
            return

        def _handler(msg) -> None:
            if node_id is None or msg.transfer.source_node_id == node_id:
                callback(data_type, msg.message)

        self.node.add_handler(data_type, _handler)

    def register_periodic_publisher(self, topic: str, field: str, frequency: float = 10.0) -> None:
        """
        Creates and registers a fast setter lambda for a given topic and field.
        Preallocates arrays if necessary.
        """
        data_type = DronecanBackend._topic_to_data_type(topic)
        if not data_type:
            return

        if not isinstance(field, str):
            raise ValueError("Field must be a string")

        if topic not in self.periodic_pubs:
            self.periodic_pubs[topic] = {
                "msg": data_type(),
                "timestamp": time.time(),
                "frequency": frequency,
                "fields": {},
            }

            logger.info("Add periodic publisher for %s", topic)

        if topic == "dronecan.uavcan.equipment.esc.RawCommand":
            if len(self.periodic_pubs[topic]["msg"].cmd) == 0:
                self.periodic_pubs[topic]["msg"].cmd = [int(0)] * 8

            cmd = self.periodic_pubs[topic]["msg"].cmd

            match = re.match(r"cmd\[(\d+)\]$", field)
            if match:
                idx = int(match.group(1))
                setter = lambda val, i=idx: cmd.__setitem__(i, int(RAW_COMMAND_SCALE * val))
                self.periodic_pubs[topic]["fields"][field] = setter
                logger.info("Registered field setter for %s %s", topic, field)
            else:
                logger.error("Field setter for %s %s not implemented", topic, field)
                return
        elif topic == "dronecan.uavcan.equipment.actuator.ArrayCommand":
            if field == "commands[0].actuator_id = 10 | commands[0].command_value":
                self.periodic_pubs[topic]["msg"].commands.append(
                    dronecan.uavcan.equipment.actuator.Command(actuator_id=10, command_value=0.0)
                )
                setter = lambda val, arr=self.periodic_pubs[topic]["msg"].commands: arr.__setitem__(0, dronecan.uavcan.equipment.actuator.Command(actuator_id=10, command_value=float(val)))
            elif field == "commands[1].actuator_id = 8 | commands[1].command_value":
                self.periodic_pubs[topic]["msg"].commands.append(
                    dronecan.uavcan.equipment.actuator.Command(actuator_id=8, command_value=-1.0)
                )
                setter = lambda val, arr=self.periodic_pubs[topic]["msg"].commands: arr.__setitem__(1, dronecan.uavcan.equipment.actuator.Command(actuator_id=8, command_value=float(val)))
            self.periodic_pubs[topic]["fields"][field] = setter
        elif topic == "dronecan.uavcan.equipment.hardpoint.Command" and field == "command":
            setter = lambda val: setattr(self.periodic_pubs[topic]["msg"], field, int(val))
            self.periodic_pubs[topic]["fields"][field] = setter
        else:
            logger.error("Field setter for %s %s not implemented", topic, field)


    def update_periodic_publisher(self, topic: str, field: str, value) -> None:
        """
        Updates the given periodic publisher's field with a new value.
        """
        if topic not in self.periodic_pubs:
            logger.warning("Topic '%s' not registered for periodic publishing", topic)
            return

        if field not in self.periodic_pubs[topic]["fields"]:
            logger.warning("Field '%s' not registered for topic '%s'", field, topic)
            return

        self.periodic_pubs[topic]["fields"][field](value)

    def read_param(self, param_name: str, node_id: Union[int, None]) -> Union[bool, int, float, str, None]:
        """Get DroneCAN parameter by name."""
        def _callback(msg: dronecan.uavcan.protocol.param.GetSet.Response):
            self._param_value = self._param_value = DronecanBackend._getset_response_to_value(msg)

        req = dronecan.uavcan.protocol.param.GetSet.Request()
        req.name = param_name
        self._param_value = None
        self.node.request(req, node_id, _callback)
        self.tick(0.01)
        return self._param_value

    def write_param(self, param_name: str, value, node_id: Union[int, None]) -> Union[bool, int, float, str, None]:
        """Set DroneCAN parameter by name."""
        def _callback(msg: dronecan.uavcan.protocol.param.GetSet.Response):
            self._param_value = DronecanBackend._getset_response_to_value(msg)

        req = dronecan.uavcan.protocol.param.GetSet.Request()
        req.name = param_name

        if isinstance(value, int):
            req.value.integer_value = value
        elif isinstance(value, str):
            req.value.string_value = value

        self._param_value = None
        for attempt in range(3):
            self.node.request(req, node_id, _callback)
            self.tick(0.01)
            if self._param_value:
                break

        logger.error("write_param", attempt, param_name, value, node_id)
        return self._param_value

    def get_info(self, node_id: int):
        """Get DroneCAN node info."""
        self._info = {}

        def callback(transfer: dronecan.node.TransferEvent):
            if transfer is None:
                return

            self._info['name'] = transfer.response.name.decode('utf-8').rstrip('\x00')
            self._info['node_id'] = transfer.transfer.source_node_id

            sw_major = transfer.response.software_version.major
            sw_minor = transfer.response.software_version.minor
            vcs_commit = hex(transfer.response.software_version.vcs_commit)[2:]
            self._info['software_version'] = f"v{sw_major}.{sw_minor}-{vcs_commit}"

            hw_major = transfer.response.hardware_version.major
            hw_minor = transfer.response.hardware_version.minor
            self._info['hardware_version'] = f"v{hw_major}.{hw_minor}"

            unique_id = bytes(transfer.response.hardware_version.unique_id)
            self._info['unique_id'] = ''.join(f'{b:02X}' for b in unique_id)

        req = dronecan.uavcan.protocol.GetNodeInfo.Request()
        self.node.request(req, node_id, callback)
        self.tick(0.01)

        if not self._info:
            logger.warning("Failed to get info from node %s", node_id)

        return self._info

    def tick(self, dt: float) -> None:
        try:
            self.node.spin(0.01)
        except KeyboardInterrupt:
            logger.info("Terminated by user.")
            sys.exit(0)
        except NotImplementedError:
            logger.critical("NotImplementedError. Check python-can == 4.3 is installed.")
            sys.exit(1)
        except dronecan.transport.TransferError:
            pass
        except dronecan.driver.common.DriverError:
            logger.critical("dronecan.driver.common.DriverError")
        except ValueError as e:
            logger.error("ValueError: %s", e)
        except dronecan.driver.common.TxQueueFullError as e:
            logger.error("TxQueueFullError: %s", e)
        except serial.serialutil.SerialException as e:
            logger.critical("SerialException: %s", e)
            sys.exit(1)

        for _, pub in self.periodic_pubs.items():
            current_time = time.time()
            if current_time - pub["timestamp"] >= 1 / pub["frequency"]:
                self.node.broadcast(pub["msg"])
                pub["timestamp"] = current_time

    @staticmethod
    def _getset_response_to_value(msg: dronecan.uavcan.protocol.param.GetSet.Response) -> ParamValue:
        if msg is None:
            return
        if hasattr(msg.response.value, 'boolean_value'):
            value = bool(msg.response.value.boolean_value)
        elif hasattr(msg.response.value, 'integer_value'):
            value = int(msg.response.value.integer_value)
        elif hasattr(msg.response.value, 'real_value'):
            value = float(msg.response.value.real_value)
        elif hasattr(msg.response.value, 'string_value'):
            string = msg.response.value.string_value
            value = str(string) if len(string) > 0 and string[0] != 255 else ""
        else:
            value = None

        return value

    @staticmethod
    def _topic_to_data_type(topic: str):
        if not isinstance(topic, str):
            raise ValueError("Topic must be a string")

        if not topic.startswith("dronecan."):
            logger.debug("Skipping '%s' (not DroneCAN topic)", topic)
            return None

        try:
            parts = topic.split(".")
            module = importlib.import_module(parts[0])  # e.g. 'dronecan'
            data_type = module
            for p in parts[1:]:
                try:
                    data_type = getattr(data_type, p)
                except AttributeError:
                    raise AttributeError(f"Invalid type path: {topic}")
        except ModuleNotFoundError as e:
            logger.error("Failed to resolve topic '%s': %s", topic, e)
            return None

        return data_type

def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument(
        "-i",
        "--iface",
        type=str,
        default="slcan:/dev/ttyACM0",
        help="CAN interface, e.g. 'socketcan:can0' or 'slcan:/dev/ttyACM0@1000000'",
    )
    parser.add_argument(
        "-n",
        "--node-id",
        type=int,
        default=100,
        help="DroneCAN node ID (default: 42)",
    )
    parser.add_argument(
        "-v",
        "--verbose",
        action="store_true",
        help="Enable debug logging",
    )

    args = parser.parse_args()

    if args.verbose:
        logging.getLogger().setLevel(logging.DEBUG)

    backend = DronecanBackend(iface=args.iface, node_id=args.node_id)
    backend.subscribe("dronecan.uavcan.protocol.NodeStatus",
                      lambda topic, msg: logger.info("NodeStatus: %s: %s", topic, msg))
    while True:
        backend.tick(0.01)

if __name__ == "__main__":
    main()
