#! /usr/bin/python3


####################################################################
####################################################################
##
## AUTO-GENERATED Application SERVER
##
## you should not need to modify this file.
##
####################################################################

import rospy
import json
from actionlib.action_server import ActionServer
import hri_actions_msgs.msg
import threading

import {{ id }}.application_controller


class ApplicationControlActionServer:
    # create messages that are used to publish feedback/result
    _feedback = hri_actions_msgs.msg.ApplicationControlFeedback()
    _result = hri_actions_msgs.msg.ApplicationControlResult()

    def __init__(self, name):
        self._action_name = name
        self._as = ActionServer(
            "/applications" + self._action_name + "/control",
            hri_actions_msgs.msg.ApplicationControlAction,
            goal_cb=self.goal_cb,
            cancel_cb=self.cancel_cb,
            auto_start=False,
        )

        self.application_controller = {{ id }}.application_controller.ApplicationController()

        self._execute_thread = None
        self._is_running = False

        self._as.start()

    def goal_cb(self, goal):

        rospy.loginfo("[APPLICATION {{ id }}] requesting start")

        if not self.application_controller:
            rospy.logwarn("[APPLICATION {{ id }}] start rejected: controller not ready")
            goal.set_rejected(text="Application controller not ready")
            return


        if self._execute_thread and self._execute_thread.is_alive():
            rospy.logwarn("[APPLICATION {{ id }}] start rejected: already running")
            goal.set_rejected(text="Application already running")
            return


        self.application_controller.cancellation_requested = False

        params = None
        if goal.get_goal().parameters:
            try:
                params = json.loads(goal.get_goal().parameters)
            except json.JSONDecodeError as je:
                rospy.logwarn("[APPLICATION {{ id }}] start rejected: invalid JSON parameters")
                goal.set_rejected(text=str(je))
                return

        self._execute_thread = threading.Thread(target=self.application_controller.run, args=(params,))

        try:
            self._execute_thread.start()
            goal.set_accepted()
        except Exception as e:
            rospy.logwarn("[APPLICATION {{ id }}] unexpected error: %s" % e)
            self._execute_thread.join()
            goal.set_aborted(text=str(e))
            return


    def cancel_cb(self, goal):

        if not self._execute_thread or not self._execute_thread.is_alive():
            goal.set_canceled(text="application not running")

        rospy.logwarn("[APPLICATION {{ id }}] requesting cancelation")
        self.application_controller.cancellation_requested = True

        self._execute_thread.join()

        goal.set_canceled(text="Successfully stopped the application")
        rospy.loginfo("[APPLICATION {{ id }}] successfully stopped")

if __name__ == "__main__":

    rospy.init_node("{{ id }}")

    server = ApplicationControlActionServer(rospy.get_name())
    rospy.spin()
