# 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.

from rclpy.lifecycle import Node
from rclpy.lifecycle import State
from rclpy.lifecycle import TransitionCallbackReturn
from rcl_interfaces.msg import ParameterDescriptor

from rclpy.action import ActionServer, GoalResponse

# this is an example of a custom action message generated by
# rpk. We use it to create our API to start/stop the skill
from sample_skill_msgs.action import SkillControl

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue


class SkillImpl(Node):
    """
    Implementation of {{id}}.

    This is the main class for the skill. It is a ROS2 node that uses the
    lifecycle feature of ROS2 to manage its states.

    It implements a simple 'say' skill, that outputs to the console
    messages received from the user. On an actual robot, this skill
    would use eg a TTS engine to say the message out loud.
    """

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

        # Declare ROS parameters
        #
        # You can add here any parameters that you want to be configurable
        # from the command line or from a launch file.
        #
        # You should also add the same parameters to config/00-defaults.yml
        #
        # If you do not need any parameters, you can remove this block
        self.declare_parameter(
            'parameter_name', 'parameter_value',
            ParameterDescriptor(description='A parameter for the skill')
        )

        self.get_logger().info("Initialising...")
        self._timer = None
        self._diag_pub = None
        self._diag_timer = None

        self._last_msg = ""  # for diagnostics

        self.get_logger().info('Skill {{id}} started, but not yet configured.')

    def run_skill(self, msg):

        # on an actual robot, this would be the place to call the TTS engine
        self.get_logger().warn(f"NEW MESSAGE TO USER: {msg}")
        self._last_msg = msg

    def on_request_goal(self, goal_handle):
        """Accept incoming goal if appropriate."""
        if self._state_machine.current_state[1] != "active":
            self.get_logger().error("Skill is not active yet, rejecting goal")
            return GoalResponse.REJECT

        return GoalResponse.ACCEPT

    def on_request_exec(self, goal_handle):
        """Process incoming goal."""
        self.run_skill(goal_handle.request.skill_data)

        goal_handle.succeed()
        return SkillControl.Result()

    #################################
    #
    # Lifecycle transitions callbacks
    #
    def on_configure(self, state: State) -> TransitionCallbackReturn:

        self.skill_server = ActionServer(self,
                                         SkillControl,
                                         "/say",
                                         goal_callback=self.on_request_goal,
                                         execute_callback=self.on_request_exec)

        # configure and start diagnostics publishing
        self._diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 1)
        self._diag_timer = self.create_timer(1., self.publish_diagnostics)

        self.get_logger().info("Skill {{id}} is configured, but not yet active")
        return TransitionCallbackReturn.SUCCESS

    def on_activate(self, state: State) -> TransitionCallbackReturn:
        # 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)

        self.get_logger().info("Skill {{id}} is active and running")
        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 skill...")
        self.destroy_timer(self._timer)

        self.get_logger().info("Skill {{id}} is stopped (inactive)")
        return super().on_deactivate(state)

    def on_shutdown(self, state: State) -> TransitionCallbackReturn:
        self.get_logger().info('Shutting down {{id}} skill.')

        self.skill_server.destroy()

        self.destroy_timer(self._diag_timer)
        self.destroy_publisher(self._diag_pub)

        self.get_logger().info("Skill {{id}} finalized.")
        return TransitionCallbackReturn.SUCCESS

    #################################

    def publish_diagnostics(self):

        arr = DiagnosticArray()
        msg = DiagnosticStatus(
            level=DiagnosticStatus.OK,
            name="/skills/say",
            message="skill {{id}} is running",
            values=[
                KeyValue(key="Module name", value="{{ id }}"),
                KeyValue(key="Current lifecycle state",
                         value=self._state_machine.current_state[1]),
                KeyValue(key="Last message", value=self._last_msg),
            ],
        )

        arr.header.stamp = self.get_clock().now().to_msg()
        arr.status = [msg]
        self._diag_pub.publish(arr)

    def run(self) -> None:
        """Background task of the skill."""
        pass

