#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Point, Pose, PoseArray

import actionlib

from trajectory_tracking_control.msg import ExecuteTrajectoryTrackingAction, ExecuteTrajectoryTrackingGoal

class FakeActionClient():
    def __init__(self, path, sampling_time, average_velocity):
        self.path = path
        self.t_s = sampling_time
        self.vel_avg = average_velocity

        client = actionlib.SimpleActionClient('trajectory_tracking_control', ExecuteTrajectoryTrackingAction)

        client.wait_for_server()

        goal = ExecuteTrajectoryTrackingGoal()
        goal.path = self.path_list_to_pose_array(self.path)
        goal.average_velocity = self.vel_avg
        goal.sampling_time = self.t_s

        client.send_goal(goal, feedback_cb=self.feedback_cb)

        client.wait_for_result()

        result = client.get_result()

        rospy.loginfo('Percentage: ' + str(result.distance_traveled_percentage))
        rospy.loginfo('Mission Status: ' + str(result.mission_status))
        rospy.loginfo('Outcome :' + str(result.goal_distance))
    
    def feedback_cb(self, msg):
        rospy.loginfo('Percentage: ' + str(msg.distance_traveled_percentage))
        rospy.loginfo('Mission Status: ' + str(msg.mission_status))

    def path_list_to_pose_array(self, path):
        pose_array = PoseArray()

        for (x, y) in path:
            pose = Pose()
            pose.position.x = x
            pose.position.y = y

            pose_array.poses.append(pose)

        return pose_array

if __name__ == "__main__":
    rospy.init_node('fake_action_client')
    
    # Action Goal
    path = [[2.0, 0.0], [1.8, 0.3], [1.9, 0.55], [1.7 ,0.7], [2.1, 0.94]]
    vel_avg = 0.5
    t_s = 0.1

    FakeActionClient(path, t_s, vel_avg)
