#!/usr/bin/python3.6
# Software License Agreement (BSD License)
#
# Copyright (c) 2012, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

from __future__ import annotations

import os
import sys
from typing import Any

import rclpy
from rclpy.clock import Clock, ClockType
from rclpy.node import Node

from rosapi import glob_helper, objectutils, params, proxy
from rosapi_msgs.msg import TypeDef
from rosapi_msgs.srv import (
    ActionFeedbackDetails,
    ActionGoalDetails,
    ActionResultDetails,
    ActionType,
    DeleteParam,
    GetActionServers,
    GetParam,
    GetParamNames,
    GetROSVersion,
    GetTime,
    HasParam,
    Interfaces,
    MessageDetails,
    NodeDetails,
    Nodes,
    Publishers,
    ServiceNode,
    ServiceProviders,
    ServiceRequestDetails,
    ServiceResponseDetails,
    Services,
    ServicesForType,
    ServiceType,
    SetParam,
    Subscribers,
    Topics,
    TopicsAndRawTypes,
    TopicsForType,
    TopicType,
)


class Rosapi(Node):
    NAME = "rosapi"

    def __init__(self) -> None:
        super().__init__(self.NAME)
        self.declare_parameter("topics_glob", "[*]")
        self.declare_parameter("services_glob", "[*]")
        self.declare_parameter("params_glob", "[*]")
        self.declare_parameter("params_timeout", 5.0)
        self.declare_parameter("use_private_service_names", False)
        self.globs = self.get_globs()
        self.register_services()

    # Initialises the ROS node
    def register_services(self) -> None:
        proxy.init(self)
        if self.get_namespace() == "/":
            full_name = self.get_namespace() + self.get_name()
        else:
            full_name = self.get_namespace() + "/" + self.get_name()

        timeout_sec = self.get_parameter("params_timeout").value
        params.init(full_name, timeout_sec)

        use_private_service_names = (
            self.get_parameter("use_private_service_names").get_parameter_value().bool_value
        )
        if not use_private_service_names:
            if self.get_namespace() != "/":
                self.get_logger().warn(
                    "You are running rosapi node under a namespace. The services will still use "
                    "global /rosapi/ namespace. Starting from ROS Jazzy, the services will be "
                    "namespaced under the node namespace. To use the new namespaced services, set "
                    "the parameter 'use_private_service_names' to true."
                )
            service_prefix = "/rosapi/"
        else:
            service_prefix = "~/"

        self.create_service(Topics, service_prefix + "topics", self.get_topics)
        self.create_service(Interfaces, service_prefix + "interfaces", self.get_interfaces)
        self.create_service(
            TopicsForType, service_prefix + "topics_for_type", self.get_topics_for_type
        )
        self.create_service(
            TopicsAndRawTypes,
            service_prefix + "topics_and_raw_types",
            self.get_topics_and_raw_types,
        )
        self.create_service(Services, service_prefix + "services", self.get_services)
        self.create_service(
            ServicesForType, service_prefix + "services_for_type", self.get_services_for_type
        )
        self.create_service(Nodes, service_prefix + "nodes", self.get_nodes)
        self.create_service(NodeDetails, service_prefix + "node_details", self.get_node_details)
        self.create_service(
            GetActionServers, service_prefix + "action_servers", self.get_action_servers
        )
        self.create_service(ActionType, service_prefix + "action_type", self.get_action_type)
        self.create_service(TopicType, service_prefix + "topic_type", self.get_topic_type)
        self.create_service(ServiceType, service_prefix + "service_type", self.get_service_type)
        self.create_service(Publishers, service_prefix + "publishers", self.get_publishers)
        self.create_service(Subscribers, service_prefix + "subscribers", self.get_subscribers)
        self.create_service(
            ServiceProviders, service_prefix + "service_providers", self.get_service_providers
        )
        self.create_service(ServiceNode, service_prefix + "service_node", self.get_service_node)
        self.create_service(
            MessageDetails, service_prefix + "message_details", self.get_message_details
        )
        self.create_service(
            ServiceRequestDetails,
            service_prefix + "service_request_details",
            self.get_service_request_details,
        )
        self.create_service(
            ServiceResponseDetails,
            service_prefix + "service_response_details",
            self.get_service_response_details,
        )
        self.create_service(
            ActionGoalDetails,
            service_prefix + "action_goal_details",
            self.get_action_goal_details,
        )
        self.create_service(
            ActionResultDetails,
            service_prefix + "action_result_details",
            self.get_action_result_details,
        )
        self.create_service(
            ActionFeedbackDetails,
            service_prefix + "action_feedback_details",
            self.get_action_feedback_details,
        )
        self.create_service(SetParam, service_prefix + "set_param", self.set_param)
        self.create_service(GetParam, service_prefix + "get_param", self.get_param)
        self.create_service(HasParam, service_prefix + "has_param", self.has_param)
        self.create_service(DeleteParam, service_prefix + "delete_param", self.delete_param)
        self.create_service(GetParamNames, service_prefix + "get_param_names", self.get_param_names)
        self.create_service(GetTime, service_prefix + "get_time", self.get_time)
        self.create_service(GetROSVersion, service_prefix + "get_ros_version", self.get_ros_version)

    def get_globs(self) -> glob_helper.Globs:
        return glob_helper.get_globs(self)

    def get_topics(self, _request: Topics.Request, response: Topics.Response) -> Topics.Response:
        """
        Return a list of all the topics being published.

        Called by the rosapi/Topics service.
        """
        response.topics, response.types = proxy.get_topics_and_types(self.globs.topics)
        return response

    def get_interfaces(
        self, _request: Interfaces.Request, response: Interfaces.Response
    ) -> Interfaces.Response:
        """
        Return a list of all the interfaces in the system.

        Called by the rosapi/Interfaces service.
        """
        response.interfaces = proxy.get_interfaces()
        return response

    def get_topics_for_type(
        self, request: TopicsForType.Request, response: TopicsForType.Response
    ) -> TopicsForType.Response:
        """
        Return a list of all the topics that are publishing a given type.

        Called by the rosapi/TopicsForType service.
        """
        response.topics = proxy.get_topics_for_type(request.type, self.globs.topics)
        return response

    def get_topics_and_raw_types(
        self, _request: TopicsAndRawTypes.Request, response: TopicsAndRawTypes.Response
    ) -> TopicsAndRawTypes.Response:
        """
        Return a list of all the topics being published, and their raw types.

        Similar to `gendeps --cat`.

        Called by the rosapi/TopicsAndRawTypes service.
        """
        response.topics, response.types = proxy.get_topics_and_types(self.globs.topics)
        response.typedefs_full_text = [
            objectutils.get_typedef_full_text(type_name) for type_name in response.types
        ]
        return response

    def get_services(
        self, _request: Services.Request, response: Services.Response
    ) -> Services.Response:
        """
        Return a list of all the services being advertised.

        Called by the rosapi/Services service.
        """
        response.services = proxy.get_services(self.globs.services)
        return response

    def get_services_for_type(
        self, request: ServicesForType.Request, response: ServicesForType.Response
    ) -> ServicesForType.Response:
        """
        Return a list of all the services that are publishing a given type.

        Called by the rosapi/ServicesForType service.
        """
        response.services = proxy.get_services_for_type(request.type, self.globs.services)
        return response

    def get_nodes(self, _request: Nodes.Request, response: Nodes.Response) -> Nodes.Response:
        """
        Return a list of all the nodes that are registered.

        Called by the rosapi/Nodes service.
        """
        response.nodes = proxy.get_nodes()
        return response

    def get_node_details(
        self, request: NodeDetails.Request, response: NodeDetails.Response
    ) -> NodeDetails.Response:
        """
        Return a node description.

        Called by the rosapi/Nodes service.
        """
        (
            response.subscribing,
            response.publishing,
            response.services,
        ) = proxy.get_node_info(request.node)
        return response

    def get_action_servers(
        self, _request: GetActionServers.Request, response: GetActionServers.Response
    ) -> GetActionServers.Response:
        """
        Return a list of action servers based on actions standard topics.

        Called by the rosapi/GetActionServers service.
        """
        topics = proxy.get_topics(self.globs.topics, include_hidden=True)
        response.action_servers = proxy.filter_action_servers(topics)
        return response

    def get_topic_type(
        self, request: TopicType.Request, response: TopicType.Response
    ) -> TopicType.Response:
        """
        Given the name of a topic, return the name of the type of that topic.

        Request class has one field, 'topic', which is a string value (the name of the topic).
        Response class has one field, 'type', which is a string value (the type of the topic).
        If the topic does not exist, an empty string is returned.

        Called by the rosapi/TopicType service.
        """
        response.type = proxy.get_topic_type(request.topic, self.globs.topics)
        return response

    def get_service_type(
        self, request: ServiceType.Request, response: ServiceType.Response
    ) -> ServiceType.Response:
        """
        Given the name of a service, return the type of that service.

        Request class has one field, 'service', which is a string value (the name of the service).
        Response class has one field, 'type', which is a string value (the type of the service).
        If the service does not exist, an empty string is returned.

        Called by the rosapi/ServiceType service.
        """
        response.type = proxy.get_service_type(request.service, self.globs.services)
        return response

    def get_publishers(
        self, request: Publishers.Request, response: Publishers.Response
    ) -> Publishers.Response:
        """
        Given the name of a topic, return a list of node names that are publishing on that topic.

        Called by the rosapi/Publishers service.
        """
        response.publishers = proxy.get_publishers(request.topic, self.globs.topics)
        return response

    def get_subscribers(
        self, request: Subscribers.Request, response: Subscribers.Response
    ) -> Subscribers.Response:
        """
        Given the name of a topic, return a list of node names that are subscribing to that topic.

        Called by the rosapi/Subscribers service.
        """
        response.subscribers = proxy.get_subscribers(request.topic, self.globs.topics)
        return response

    def get_service_providers(
        self, request: ServiceProviders.Request, response: ServiceProviders.Response
    ) -> ServiceProviders.Response:
        """
        Given the name of a topic, returns a list of node names that are advertising that service type.

        Called by the rosapi/ServiceProviders service.
        """
        response.providers = proxy.get_service_providers(request.service, self.globs.services)
        return response

    def get_service_node(
        self, request: ServiceNode.Request, response: ServiceNode.Response
    ) -> ServiceNode.Response:
        """
        Given the name of a service, returns the name of the node that is providing that service.

        Called by the rosapi/ServiceNode service.
        """
        response.node = proxy.get_service_node(request.service, self.globs.services)
        return response

    def get_message_details(
        self, request: MessageDetails.Request, response: MessageDetails.Response
    ) -> MessageDetails.Response:
        """
        Given the name of a message type, return the TypeDef for that type.

        Called by the rosapi/MessageDetails service.
        """
        response.typedefs = [
            dict_to_typedef(d) for d in objectutils.get_typedef_recursive(request.type)
        ]
        return response

    def get_service_request_details(
        self, request: ServiceRequestDetails.Request, response: ServiceRequestDetails.Response
    ) -> ServiceRequestDetails.Response:
        """
        Given the name of a service type, return the TypeDef for the request message of that service type.

        Called by the rosapi/ServiceRequestDetails service.
        """
        response.typedefs = [
            dict_to_typedef(d)
            for d in objectutils.get_service_request_typedef_recursive(request.type)
        ]
        return response

    def get_service_response_details(
        self, request: ServiceResponseDetails.Request, response: ServiceResponseDetails.Response
    ) -> ServiceResponseDetails.Response:
        """
        Given the name of a service type, return the TypeDef for the response message of that service type.

        Called by the rosapi/ServiceResponseDetails service.
        """
        response.typedefs = [
            dict_to_typedef(d)
            for d in objectutils.get_service_response_typedef_recursive(request.type)
        ]
        return response

    def get_action_goal_details(
        self, request: ActionGoalDetails.Request, response: ActionGoalDetails.Response
    ) -> ActionGoalDetails.Response:
        """
        Given the name of an action type, return the TypeDef for the goal message of that action type.

        Called by the rosapi/ActionGoalDetails service.
        """
        response.typedefs = [
            dict_to_typedef(d) for d in objectutils.get_action_goal_typedef_recursive(request.type)
        ]
        return response

    def get_action_result_details(
        self, request: ActionResultDetails.Request, response: ActionResultDetails.Response
    ) -> ActionResultDetails.Response:
        """
        Given the name of an action type, return the TypeDef for the result message of that action type.

        Called by the rosapi/ActionResultDetails service.
        """
        response.typedefs = [
            dict_to_typedef(d)
            for d in objectutils.get_action_result_typedef_recursive(request.type)
        ]
        return response

    def get_action_feedback_details(
        self, request: ActionFeedbackDetails.Request, response: ActionFeedbackDetails.Response
    ) -> ActionFeedbackDetails.Response:
        """
        Given the name of an action type, return the TypeDef for the feedback message of that action type.

        Called by the rosapi/ActionFeedbackDetails service.
        """
        response.typedefs = [
            dict_to_typedef(d)
            for d in objectutils.get_action_feedback_typedef_recursive(request.type)
        ]
        return response

    def get_action_type(
        self, request: ActionType.Request, response: ActionType.Response
    ) -> ActionType.Response:
        """
        Given the name of an action, return its type.

        If the action does not exist, an empty string is returned.

        Called by the rosapi/ActionType service.
        """
        response.type = proxy.get_action_type(request.action)
        return response

    async def set_param(
        self, request: SetParam.Request, response: SetParam.Response
    ) -> SetParam.Response:
        try:
            node_name, param_name = self._get_node_and_param_name(request.name)
            params.set_param(node_name, param_name, request.value, self.globs.params)
        except ValueError:
            self._print_malformed_param_name_warning(request.name)
        return response

    def get_param(
        self, request: GetParam.Request, response: GetParam.Response
    ) -> GetParam.Response:
        try:
            node_name, param_name = self._get_node_and_param_name(request.name)
            response.value = params.get_param(
                node_name, param_name, request.default_value, self.globs.params
            )
        except ValueError:
            self._print_malformed_param_name_warning(request.name)
        return response

    def has_param(
        self, request: HasParam.Request, response: HasParam.Response
    ) -> HasParam.Response:
        try:
            node_name, param_name = self._get_node_and_param_name(request.name)
            response.exists = params.has_param(node_name, param_name, self.globs.params)
        except ValueError:
            self._print_malformed_param_name_warning(request.name)
        return response

    def delete_param(
        self, request: DeleteParam.Request, response: DeleteParam.Response
    ) -> DeleteParam.Response:
        params.delete_param(request.node_name, request.name, self.globs.params)
        return response

    def get_param_names(
        self, _request: GetParamNames.Request, response: GetParamNames.Response
    ) -> GetParamNames.Response:
        response.names = params.get_param_names(self.globs.params)
        return response

    def get_time(self, _request: GetTime.Request, response: GetTime.Response) -> GetTime.Response:
        response.time = Clock(clock_type=ClockType.ROS_TIME).now().to_msg()
        return response

    def _get_node_and_param_name(self, param: str) -> tuple[str, str]:
        return tuple(param.split(":"))

    def _print_malformed_param_name_warning(self, param_name: str) -> None:
        self.get_logger().warning(
            f"Malformed parameter name: {param_name}; expecting <node_name>:<param_name>"
        )

    def get_ros_version(
        self, _request: GetROSVersion.Request, response: GetROSVersion.Response
    ) -> GetROSVersion.Response:
        response.version = 2
        response.distro = str(os.environ["ROS_DISTRO"])
        return response


def dict_to_typedef(typedefdict: dict[str, Any]) -> TypeDef:
    typedef = TypeDef()
    typedef.type = typedefdict["type"]
    typedef.fieldnames = typedefdict["fieldnames"]
    typedef.fieldtypes = typedefdict["fieldtypes"]
    typedef.fieldarraylen = typedefdict["fieldarraylen"]
    typedef.examples = typedefdict["examples"]
    typedef.constnames = typedefdict["constnames"]
    typedef.constvalues = typedefdict["constvalues"]
    return typedef


def main(args: list[str] | None = None) -> None:
    if args is None:
        args = sys.argv

    rclpy.init(args=args)
    node = Rosapi()
    try:
        rclpy.spin(node)
        node.destroy_node()
        rclpy.shutdown()
    except KeyboardInterrupt:
        print("Exiting due to SIGINT")


if __name__ == "__main__":
    main()
