#!/usr/bin/env python3

import actionlib
import rospy

# import msgs
import aruco.msg

class ActionClient:
    def __init__(self):

        ### launch parameters
        try:
            GOAL_ID = int(rospy.get_param('goal_id'))
        except Exception:
            print('Cant load Aruco parameters from ROS parameter server ...')
            GOAL_ID = 0

        print(f'Goal ID: {GOAL_ID}')
        
        ### action client definition
        # set action name
        action_name = 'detectMarker'
        
        # set action definition - ros msg
        action_definition = aruco.msg.detectMarkerAction
        
        # set action goal - ros msg
        self.goal = aruco.msg.detectMarkerGoal()
        self.goal.id = GOAL_ID
        
        ### create action client and wait for server connection
        self.client = actionlib.SimpleActionClient(action_name, action_definition)
        self.client.wait_for_server()
        
        ### send goal to server and define callbacks functions
        self.client.send_goal(self.goal, active_cb=self.active, feedback_cb=self.feedback, done_cb=self.done)
        
    '''
    Callback Active - is called when action starts successfully
    '''
    def active(self):
        print('Active mode starts ...')

    '''
    Callback feedback - is called when action server sends feedback ... result is ros feedback msg
    '''
    def feedback(self, feedback):
        print('Feedback received ... '+str(feedback))
        
        # # some code here - if you ready remove it!
        # if feedback.percent_complete == 10:
        #     ###cancel active goal!!!
        #     self.client.cancel_goal()
        
    '''
    Callback done - is called when action is done successfully (state == 3) or not (state == 2/4)... result is ros result msg
    '''
    def done(self, state, result):
        print('Done, result received ... '+str(result))

if __name__ == '__main__':
    node_name = 'my_action_client'
    rospy.init_node(node_name)
    server = ActionClient()
    rospy.spin()

