# Copyright (c) {{ year }} {{ author }}. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import json

from rclpy.lifecycle import Node
from rclpy.lifecycle import State
from rclpy.lifecycle import TransitionCallbackReturn

from ament_index_python.packages import get_package_share_directory
from task_msgs.action import TaskControl
from ui_msgs.srv import SetUiFragment

from rclpy.action import ActionClient
{% if intents %}
{% for intent in intents %}
{% if intent.intent == "ENGAGE_WITH"%}
{% if "communication" in features or "pal_arch" in features %}
from communication_skills import Say
{% endif %}
{% if "gestures" in features %}
from play_motion2_msgs.action import PlayMotion2
{% endif %}
{% if "pal_arch" not in features %}
from sample_skill_msgs.action import SkillControl
{% endif %}
{% endif %}
{% endfor %}
{% endif %}

from hri_actions_msgs.msg import Intent
{% if "pal_arch" in features %}
from lifecycle_msgs.msg import State as StateMsg
from {{ id }}.change_state_helper import ChangeNodeStateHelper
{% endif %}
from {{ id }}.i18n import I18nSupport


# set by I18Support in on_configure()
def _(s): return s


class MissionController(Node):

    def __init__(self) -> None:
        """Construct the node."""
        super().__init__('app_{{ id }}')

        self.get_logger().info("Initialising...")
        self._intents_sub = None
        self._timer = None

{% if "pal_arch" in features %}
        # We manage the state of the "communication_hub" node
        self._change_state_helper = ChangeNodeStateHelper(
            self,
            'communication_hub')
{% endif %}
        # helper class to handle locale changes
        self.i18n = I18nSupport(self, '{{ id }}')

        self.ui_client = None  # service client to set the UI

        #######################################
        #
        # TODO: Add here any other variables
        # that you need in your node
        # Initialisation should happen in the
        # on_configure state.
        #

        self.inc = None

        self.get_logger().info('Mission controller initialized, but not yet configured.')

    def set_ui(self, qml: str) -> None:
        ui_fragment = SetUiFragment.Request()
        ui_fragment.qml_import_path = get_package_share_directory("{{id}}") + "/.."
        ui_fragment.qml_fragment = "import {{id}}.ui 1.0\n" + qml

        self.ui_client.call_async(ui_fragment)

    def on_intent(self, msg):

        self.get_logger().info("Received an intent: %s" % msg.intent)

        #######################################
        #
        # TODO: Define here how your application
        # should react when receiving an user
        # intent.
        #

        data = json.loads(msg.data) if msg.data else {}  # noqa: F841
        source = msg.source  # noqa: F841
        modality = msg.modality  # noqa: F841
        confidence = msg.confidence  # noqa: F841
        priority_hint = msg.priority  # noqa: F841

        #######################################
        #
        # If you do not need to process specific
        # intents, simply remove the corresponding
        # line(s)
        #
{% if intents %}{% set ns = namespace(first_intent = true) %}
{% for intent in intents %}
    {% if ("manipulation" in features and intent.intent in ["GRAB_OBJECT", "BRING_OBJECT", "PLACE_OBJECT"]) or
          (("social" in features or "pal_arch" not in features) and intent.intent in ["ENGAGE_WITH"]) or
          (("gestures" in features or "pal_arch" not in features) and intent.intent in ["PERFORM_MOTION"]) or
          ("navigation" in features and intent.intent in ["MOVE_TO", "GUIDE"]) or
          (robot == "ari" and intent.intent in ["PRESENT_CONTENT"]) or
          (intent.intent not in ["PRESENT_CONTENT", "ENGAGE_WITH", "PERFORM_MOTION", "GRAB_OBJECT", "BRING_OBJECT", "PLACE_OBJECT", "MOVE_TO", "GUIDE"]) %}

        {{ "if" if ns.first_intent else "elif"}} msg.intent == Intent.{{ intent.intent }}:
{% if ns.first_intent %}{% set ns.first_intent = false %}{% endif %}
{% if intent.required_thematic_roles %}
            # the 'data' dictionary should contain at least the following keys:
{% for role in intent.required_thematic_roles %}
            # - {{ role }}
{% endfor %}

{% endif %}
{% if intent.intent == "WAKEUP" %}
            # Handle intents to wakeup the robot
    {% if "pal_arch" in features %}
            # The "communication_hub" is a managed node that needs to be
            # activated based on the received wakeup intent.
            self._change_state_helper.change_state(StateMsg.PRIMARY_STATE_ACTIVE)
    {% else %}
            pass
    {% endif %}
{% elif intent.intent == "SUSPEND" %}
            # Handle intents to suspend the robot
    {% if "pal_arch" in features %}
            # The "communication_hub" is a managed node that needs to be
            # deactivated based on the received suspend intent.
            self._change_state_helper.change_state(StateMsg.PRIMARY_STATE_INACTIVE)
    {% else %}
            pass
    {% endif %}
{% elif intent.intent == "ENGAGE_WITH" and ("social" in features) %}
            {% if "communication" in features %}
            # As an example, we call here the Say and play_motion action server
            # to implement a very simple behaviour when someone engages with
            # the robot.
            say = ActionClient(self, Say, 'say')
            say.wait_for_server()
            say_goal = Say.Goal()
            say_goal.input = _("engage with msg")
            say.send_goal_async(say_goal)
            {% endif %}
            {% if "gestures" in features %}

            play_motion = ActionClient(self, PlayMotion2, 'play_motion2')
            play_motion.wait_for_server()
            {% if robot == "ari" %}
            motion_goal = PlayMotion2.Goal(motion_name="bow")
            {% elif robot == "tiago" %}
            motion_goal = PlayMotion2.Goal(motion_name="head_tour")
            play_motion.send_goal_async(motion_goal)
            {% endif %}
            {% endif %}
{% elif intent.intent in ["GRAB_OBJECT", "BRING_OBJECT", "PLACE_OBJECT"] %}
            # Handle intents to manipulate an object. Refer to "manipulation
            # skills":
            # https://docs.pal-robotics.com/sdk-dev/manipulation/skills_list
            pass
{% elif intent.intent in ["PRESENT_CONTENT"] %}
            # Handle intents to present content to the user: information
            # about a product, a service, etc.
            pass
{% elif intent.intent in ["MOVE_TO", "GUIDE"] %}
            # Handle intents to move the robot from one place to another using
            # its navigation skills. Refer to "navigation skills":
            # https://docs.pal-robotics.com/sdk-dev/navigation/skills_list
            pass
{% elif intent.intent == "PERFORM_MOTION" %}
            # Handle intents to perform a specific motion
            # (eg, a dance or a specific gesture like pointing, waving)
            # Refer to "gestures and motions skills":
            # https://docs.pal-robotics.com/sdk-dev/motions/skills_list
            {% if "gestures" in features %}
            play_motion = ActionClient(self, PlayMotion2, 'play_motion2')
            play_motion.wait_for_server()
            motion_goal = PlayMotion2.Goal(motion_name=data['object'])
            play_motion.send_goal(motion_goal)
            {% else %}
            pass
            {% endif %}
{% elif intent.intent in ["GREET"] and "pal_arch" in features %}
            # Handle intents to greet, customised by the user
            # Refer to:
            # https://docs.pal-robotics.com/sdk-dev/speech/skills_list
            say = ActionClient(self, Say, 'say')
            say.wait_for_server()
            {% if robot != "generic-pal" %}
            say_goal = Say.Goal(input=_("Hello, I'm {{robot_name}}. Nice to meet you!"))
            {% else %}
            say_goal = Say.Goal(input=_("Hello. Nice to meet you!"))
            {% endif %}
            say.send_goal_async(say_goal)
{% elif intent.intent in ["GREET", "ENGAGE_WITH"] and "pal_arch" not in features %}
            # Handle intents to greet, customised by the user
            # Refer to:
            # https://docs.pal-robotics.com/sdk-dev/speech/skills_list
            say = ActionClient(self, SkillControl, '/say')
            say.wait_for_server()
            say_goal = SkillControl.Goal(skill_data=_("Hello, what can I do for you today?"))
            say.send_goal_async(say_goal)
{% elif intent.intent in ["SAY"] and "pal_arch" in features %}
            # Handle intents to say a specific text
            # Refer to:
            # https://docs.pal-robotics.com/sdk-dev/speech/skills_list
            say = ActionClient(self, Say, 'say')
            say.wait_for_server()
            say_goal = Say.Goal(input=data['object'])
            say.send_goal_async(say_goal)
{% elif intent.intent == "SAY" and "pal_arch" not in features %}
            # Handle intents to greet, customised by the user
            # Refer to:
            # https://docs.pal-robotics.com/sdk-dev/speech/skills_list
            say = ActionClient(self, SkillControl, '/say')
            say.wait_for_server()
            say_goal = SkillControl.Goal(skill_data=data['object'])
            say.send_goal_async(say_goal)
{% elif intent.intent == "START_ACTIVITY" %}
            if "object" in data:
                task = data["object"]
                self.start_task(task)
            else:
                self.get_logger().error("Received intent to start activity without object")
{% elif intent.intent == "STOP_ACTIVITY" %}
            if "object" in data:
                self.get_logger().info("Received intent to stop activity: %s" % data["object"])
            else:
                self.get_logger().error("Received intent to stop activity without object")
{% endif %}
{% endif %}
{% endfor %}

        # custom intent?
        else:
            self.get_logger().warning("I don't know how to process intent "
                                      "<%s>!" % msg.intent)
{% else %}
        # handle here the different intents
        if msg.intent == "your_intent":  # or one of the Intent.msg constants
            pass

        # elif...

        else:
            self.get_logger().warning("I don't know how to process intent "
                                      "<%s>!" % msg.intent)
{% endif %}

    def start_task(self, task: str) -> None:

        if task == "sample_task_1":
            self.get_logger().info("Starting task: %s" % task)

            # create an action client to call the tasks' action server
            # and send a goal to start the task
            task_client = ActionClient(self, TaskControl, '/sample_gui_task/control')
            task_ok = task_client.wait_for_server(timeout_sec=1)
            if not task_ok:
                self.get_logger().error("The task server is not available. Task not started?")
            else:
                task_goal = TaskControl.Goal()
                task_goal.task_data = "go go go"
                task_client.send_goal_async(task_goal)
        else:
            self.get_logger().warn("Task %s not implemented" % task)
            self.set_ui("MainPage{"
                        "alertMsg: 'Task %s not yet implemented';"
                        "popupVisible:true}" % task)

    #########################################################################
    #################################
    #
    # Lifecycle transitions callbacks
    #
    def on_configure(self, state: State) -> TransitionCallbackReturn:
        """
        Configure the node.

        :return: The state machine either invokes a transition to the
            "inactive" state or stays in "unconfigured" depending on the
            return value.
            TransitionCallbackReturn.SUCCESS transitions to "inactive".
            TransitionCallbackReturn.FAILURE transitions to "unconfigured".
            TransitionCallbackReturn.ERROR or any uncaught exceptions to
            "errorprocessing"
        """
        # set the global '_' function to the translation function
        global _
        self.i18n.configure()
        _ = self.i18n.tr

        self.inc = 0
        self.get_logger().info('Mission controller configured, but not yet active.')
        return TransitionCallbackReturn.SUCCESS

    def on_activate(self, state: State) -> TransitionCallbackReturn:
        """
        Activate the node.

        Subscribe to topic and start listening to /intents topic.
        Activate the 'communication_hub' node.
        """
        self._intents_sub = self.create_subscription(
            Intent,
            '/intents',
            self.on_intent,
            10)
        self.get_logger().info("Listening to %s topic" %
                               self._intents_sub.topic_name)

        ui_service = "/ui/set_fragment"
        self.ui_client = self.create_client(SetUiFragment, ui_service)
        ui_ok = self.ui_client.wait_for_service(timeout_sec=1)
        if not ui_ok:
            self.get_logger().error(f"The UI server is not available (service {ui_service}).")
        else:
            self.set_ui("MainPage {}")

        # Define a timer that fires every second to call the run function
        timer_period = 1  # in sec
        self._timer = self.create_timer(timer_period, self.run)

{% if "pal_arch" in features %}
        # We activate the "communication_hub" node by default
        self._change_state_helper.change_state(StateMsg.PRIMARY_STATE_ACTIVE)

{% endif %}
        self.get_logger().info('Mission controller active.')
        return super().on_activate(state)

    def on_deactivate(self, state: State) -> TransitionCallbackReturn:
        """Stop the timer to stop calling the `run` function (main task of your application)."""
        self.get_logger().info("Stopping application")

        self.destroy_client(self.ui_client)
        self.destroy_timer(self._timer)
        self.destroy_subscription(self._intents_sub)

        self.get_logger().info('Mission controller deactivated.')
        return super().on_deactivate(state)

    def on_shutdown(self, state: State) -> TransitionCallbackReturn:
        """
        Shutdown the node, after a shutting-down transition is requested.

        :return: The state machine either invokes a transition to the
            "finalized" state or stays in the current state depending on the
            return value.
            TransitionCallbackReturn.SUCCESS transitions to "finalized".
            TransitionCallbackReturn.FAILURE remains in current state.
            TransitionCallbackReturn.ERROR or any uncaught exceptions to
            "errorprocessing"
        """
        self.i18n.shutdown()

        self.get_logger().info('Mission controller shutting down now.')
        return TransitionCallbackReturn.SUCCESS

    def run(self) -> None:
        """
        Background processing of your applcation.

        The main task of your application. This function will be triggered by
        the timer in the class, and only when the timer is active.
        """
        #######################################
        #
        # TODO: Implement here any background
        # task required by your mission controller
        # If application has been stopped,
        # this function will not be called
        #

        self.get_logger().info(str(self.inc))
        self.inc += 1

