#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import PoseStamped

class ArmController(Node):
    def __init__(self):
        super().__init__('arm_point_controller')
        
        # Create an action client for the MoveGroup action server
        self._action_client = ActionClient(self, MoveGroup, '/move_action')
        self.get_logger().info("Waiting for move_group action server...")
        self._action_client.wait_for_server()
        self.get_logger().info("Connected to move_group server!")

    def send_goal(self):
        goal_msg = MoveGroup.Goal()
        
        # 1. Configure general planning workspace/group settings
        goal_msg.request.group_name = "arm"
        goal_msg.request.allowed_planning_time = 5.0
        goal_msg.request.num_planning_attempts = 5
        goal_msg.request.pipeline_id = "ompl"
        
        # 2. Define our target pose
        target_pose = PoseStamped()
        target_pose.header.frame_id = "world"
        target_pose.pose.position.x = -0.3
        target_pose.pose.position.y = 1.0
        target_pose.pose.position.z = 0.9

        target_pose.pose.orientation.x = 0.0
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.z = 0.0
        target_pose.pose.orientation.w = 1.0

        # 3. Package the target pose into MoveIt's expected constraints format
        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = "world"
        position_constraint.link_name = "tool_link" # Make sure this matches your tip link name!
        
        # Define a small target volume region around the point (0.01m tolerance)
        box = SolidPrimitive()
        box.type = SolidPrimitive.BOX
        box.dimensions = [0.01, 0.01, 0.01]
        
        volume = BoundingVolume()
        volume.primitives.append(box)
        volume.primitive_poses.append(target_pose.pose)
        position_constraint.constraint_region = volume
        position_constraint.weight = 1.0

        # Bundle constraints together
        goal_constraints = Constraints()
        goal_constraints.position_constraints.append(position_constraint)
        goal_msg.request.goal_constraints.append(goal_constraints)

        # 4. Fire the goal request over to MoveIt
        self.get_logger().info("Sending goal coordinates to MoveIt...")
        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error("Goal rejected by MoveIt planning pipeline!")
            return

        self.get_logger().info("Goal accepted! Planning and executing path...")
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info("Trajectory Execution Finished!")
        rclpy.shutdown()

def main(args=None):
    rclpy.init(args=args)
    node = ArmController()
    node.send_goal()
    rclpy.spin(node)

if __name__ == '__main__':
    main()