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

from rclpy.action import ActionServer, GoalResponse
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

# custom 'control' action for this task. to start/stop it.
# This has been generated by rpk alongside this node.
from task_msgs.action import TaskControl
from ui_msgs.srv import SetUiFragment

from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue


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

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

    This template does not implement any specific task, but it is a good
    starting point for creating a new task.

    Don't forget to update accordingly the manifest of the task, included in
    package.xml, to describe what your task is actually doing.
    """

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

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

        self.task_server = None  # action server to start/stop this task

        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.task_running = False
        self.completed = 0

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

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

        self.get_logger().info("Accepted a new goal")
        return GoalResponse.ACCEPT

    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_request_exec(self, goal_handle):
        """Process incoming goal."""
        task_data = goal_handle.request.task_data

        self.get_logger().info(f"Starting the task with data <{task_data}>")

        feedback_msg = TaskControl.Feedback()
        feedback_msg.status = "task started"

        goal_handle.publish_feedback(feedback_msg)

        self.task_running = True

        # set the UI: pass a QML fragment to be displayed
        self.set_ui("MainPage {}")
        feedback_msg.status = "UI set to display main QML widget"
        goal_handle.publish_feedback(feedback_msg)

        # publish feedback
        previous_completed = 0

        while self.completed != 100:

            if self.completed != previous_completed:

                self.get_logger().info(
                    f"Completed {self.completed}% of the task")
                feedback_msg.status = f"completed {self.completed}%"
                goal_handle.publish_feedback(feedback_msg)
                previous_completed = self.completed

            time.sleep(0.1)

        feedback_msg.status = "task completed"
        goal_handle.publish_feedback(feedback_msg)

        self.completed = 0
        self.task_running = False

        goal_handle.succeed()
        return TaskControl.Result(result="task completed")

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

        You usually want to do the following in this state:
        - Read ROS parameters (if any)
        - Create ROS action clients and servers
        - Create ROS publishers and subscribers
        - Start publishing diagnostic information

        While the task is configured, but not activated, it should not
        perform any actions that are not required for configuration, such as
        effectively processing data or calling external services.
        For instance, incoming goals on an action server should be rejected.

        :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"
        """
        # configure and start diagnostics publishing
        self._diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 1)
        self._diag_timer = self.create_timer(1., self.publish_diagnostics)

        # create here publishers, subscribers, clients, servers, etc.
        # required to implement the task (if any)

        # create the control server for ourselves
        self.task_server = ActionServer(
            self, TaskControl, "/{{id}}/control",
            goal_callback=self.on_request_goal,
            execute_callback=self.on_request_exec)

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

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

        You usually want to do the following in this state:
        - Create and start any timers performing periodic tasks
        - Start processing data, and accepting action goals, if any

        """
        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}).")

        # As an example, 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("Task {{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 task...")
        self.destroy_client(self.ui_client)
        self.destroy_timer(self._timer)

        self.get_logger().info("Task {{id}} is stopped (inactive)")
        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.get_logger().info('Shutting down task {{id}}.')
        self.destroy_timer(self._diag_timer)
        self.destroy_publisher(self._diag_pub)

        self.destroy_timer(self._timer)

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

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

    def publish_diagnostics(self):

        arr = DiagnosticArray()
        msg = DiagnosticStatus(
            level=DiagnosticStatus.OK,
            name="/task_{{ id }}",
            message="task {{id}} is running",
            values=[
                KeyValue(key="Module name", value="{{ id }}"),
                KeyValue(key="Current lifecycle state",
                         value=self._state_machine.current_state[1]),
                KeyValue(key="Current task completion percentage",
                         value=f"{self.completed}"),
            ],
        )

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

        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 task
        # If the task is deactivated,
        # this function is not called anymore
        #

        if self.task_running:
            self.completed += 10

