#!/usr/bin/env python3
"""
Example of moving to a pose goal.
- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=1.0
- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=0.0
"""

from threading import Thread

import rclpy
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node

from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import Constraints, PositionConstraint
from geometry_msgs.msg import PointStamped, PoseStamped
from shape_msgs.msg import SolidPrimitive

import numpy as np
import time
import cv2

import tf2_ros
import tf2_geometry_msgs


from pymoveit2 import MoveIt2, MoveIt2State
from pymoveit2.robots import openarm as robot


class HandFollowMoveIt(Node):
    def __init__(self):
        super().__init__('hand_follow_moveit')


        self.last_sent = time.time()
        self.send_period = 0.3   # 约 3 Hz

        self.p_prev = None

        self.plan_period = 0.1       # 秒（规划频率）
        self.min_delta = 0.005        # m，手移动超过才重新规划
        self.z_min = 0.15            # 防止撞桌
        self.z_max = 0.60
        self.latest_hand_pos = None
        self.last_target = None
        self.base_frame = 'openarm_body_link0'
        # Create node for this example
 

        # Declare parameters for position and orientation
        self.declare_parameter("position", [0.5, 0.0, 0.25])
        self.declare_parameter("quat_xyzw", [0.723, 0.0, 0.69, 0.0])
        self.declare_parameter("synchronous", True)
        # If non-positive, don't cancel. Only used if synchronous is False
        self.declare_parameter("cancel_after_secs", 0.0)
        # Planner ID
        self.declare_parameter("planner_id", "RRTConnectkConfigDefault")
        # Declare parameters for cartesian planning
        self.declare_parameter("cartesian", True)
        self.declare_parameter("cartesian_max_step", 0.0025)
        self.declare_parameter("cartesian_fraction_threshold", 0.0)
        self.declare_parameter("cartesian_jump_threshold", 0.0)
        self.declare_parameter("cartesian_avoid_collisions", False)

        # Create callback group that allows execution of callbacks in parallel without restrictions
        callback_group = ReentrantCallbackGroup()

        # Create MoveIt 2 interface
        self.moveit2 = MoveIt2(
            node=self,
            joint_names=robot.joint_names(),
            base_link_name=robot.base_link_name(),
            end_effector_name=robot.end_effector_name(),
            group_name=robot.MOVE_GROUP_ARM,
            callback_group=callback_group,
        )
        self.moveit2.planner_id = (
            self.get_parameter("planner_id").get_parameter_value().string_value
        )

        # Spin the node in background thread(s) and wait a bit for initialization
        # self.executor = rclpy.executors.MultiThreadedExecutor(2)
        # self.executor.add_node(self)
        # self.executor_thread = Thread(target_pos=self.executor.spin, daemon=True, args=())
        # self.executor_thread.start()
        # self.create_rate(1.0).sleep()

        # Scale down velocity and acceleration of joints (percentage of maximum)
        self.moveit2.max_velocity = 0.5
        self.moveit2.max_acceleration = 0.5

        # Get parameters
        self.position = self.get_parameter("position").get_parameter_value().double_array_value
        self.quat_xyzw = self.get_parameter("quat_xyzw").get_parameter_value().double_array_value
        self.synchronous = self.get_parameter("synchronous").get_parameter_value().bool_value
        self.cancel_after_secs = (
            self.get_parameter("cancel_after_secs").get_parameter_value().double_value
        )
        self.cartesian = self.get_parameter("cartesian").get_parameter_value().bool_value
        self.cartesian_max_step = (
            self.get_parameter("cartesian_max_step").get_parameter_value().double_value
        )
        self.cartesian_fraction_threshold = (
            self.get_parameter("cartesian_fraction_threshold")
            .get_parameter_value()
            .double_value
        )
        self.cartesian_jump_threshold = (
            self.get_parameter("cartesian_jump_threshold")
            .get_parameter_value()
            .double_value
        )
        self.cartesian_avoid_collisions = (
            self.get_parameter("cartesian_avoid_collisions")
            .get_parameter_value()
            .bool_value
        )

        # Set parameters for cartesian planning
        self.moveit2.cartesian_avoid_collisions = self.cartesian_avoid_collisions
        self.moveit2.cartesian_jump_threshold = self.cartesian_jump_threshold

        # ====== TF ======
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
        self.zero_hand()
        time.sleep(3)

        # ====== 订阅 ======
        self.sub = self.create_subscription(
            PoseStamped,
            '/hand_3d',
            self.hand_cb,
            10
        )
        self.timer = self.create_timer(self.plan_period, self.plan_timer)

        self.get_logger().info('Hand → MoveIt Planning node started')
    # ---------- 回调 ----------
    def hand_cb(self, msg):
        try:
            hand = self.tf_buffer.transform(
                msg,
                self.base_frame,
                timeout=rclpy.duration.Duration(seconds=0.1)
            )
        except Exception:
            return
        self.get_logger().info(f"手坐标（{self.base_frame}):{hand.pose.position.x:.3f}, {hand.pose.position.y:.3f}, {hand.pose.position.z:.3f}")
        self.get_logger().info(f"手四元数（{self.base_frame}):{hand.pose.orientation.x:.3f}, {hand.pose.orientation.y:.3f}, {hand.pose.orientation.z:.3f}, {hand.pose.orientation.w:.3f}")
        self.latest_hand_pos = np.array([
            hand.pose.position.x,
            hand.pose.position.y,
            hand.pose.position.z
        ])
        self.quat_xyzw = np.array([
            hand.pose.orientation.x,
            hand.pose.orientation.y,
            hand.pose.orientation.z,
            hand.pose.orientation.w
        ])
    def zero_hand(self):
        self.send_goal([0.0, 0.15, 0.3], [1.0, 0.0, 0.0, 0.0])
    # def update_hand(self, p):
    #     """ p: np.array([x,y,z]) in base_link """

    #     # 1️⃣ 限幅（按你机械臂改）
    #     p[0] = np.clip(p[0], -0.1, 0.1)
    #     p[1] = np.clip(p[1], -0.20, 0.20)
    #     p[2] = np.clip(p[2], 0.0, 0.30)

    #     # 2️⃣ 低通滤波
    #     if self.p_prev is None:
    #         self.p_prev = p
    #         return

    #     alpha = 0.2
    #     p = alpha * p + (1 - alpha) * self.p_prev

    #     # 3️⃣ 死区
    #     if np.linalg.norm(p - self.p_prev) < 0.015:
    #         return

    #     # 4️⃣ 控制频率
    #     if time.time() - self.last_sent < self.send_period:
    #         return

    #     self.send_goal(p)
    #     self.p_prev = p
    #     self.last_sent = time.time()

    def send_goal(self, p, quat):
        # Move to pose
        self.get_logger().info(
            f"Moving to {{position: {list(p)}, quat_xyzw: {list(self.quat_xyzw)}}}"
        )
        self.moveit2.move_to_pose(
            position=p,
            quat_xyzw=quat,
            cartesian=self.cartesian,
            cartesian_max_step=self.cartesian_max_step,
            cartesian_fraction_threshold=self.cartesian_fraction_threshold,
        )
            
    def plan_timer(self):

            if self.latest_hand_pos is None:
                self.zero_hand()
                return

            # 限制 workspace
            target_pos = self.latest_hand_pos.copy()
            if np.linalg.norm(target_pos) == 0:
                self.zero_hand()
                return
            
            target_pos[0] = np.clip(target_pos[0], 0, 0.8)
            target_pos[1] = np.clip(target_pos[1], -0.2, 0.2) + 0.15
            target_pos[2] = np.clip(target_pos[2], -0.3, 0.3) + 0.3

            # target_pos[0] = 0.12
            # target_pos[2] = 0.32

            # 判断是否值得重新规划
            if self.last_target is not None:
                if np.linalg.norm(target_pos - self.last_target) < self.min_delta:
                    return

            self.last_target = target_pos.copy()
            self.send_goal(target_pos, self.quat_xyzw)
            self.get_logger().info(f"发送新目标：{target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}")

def main():
    rclpy.init()
    node = HandFollowMoveIt()
    rclpy.spin(node)
    rclpy.shutdown()
    # node.executor_thread.join()
    exit(0)


if __name__ == "__main__":
    main()
