#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from builtin_interfaces.msg import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint


class UnifiedTrajectoryPublisher(Node):
    def __init__(self, name):
        super().__init__(name)
        
        self.controller_name = "scaled_joint_trajectory_controller"  # 或 "joint_trajectory_controller" could also exchange to oother controller
        self.wait_sec_between_publish = 6
        self.goal_name = ["pos1", "pos2"] # can also add pos3, 4 and so on
        self.joints = [
            "shoulder_pan_joint",
            "shoulder_lift_joint",
            "elbow_joint",
            "wrist_1_joint",
            "wrist_2_joint",
            "wrist_3_joint"
        ]
        self.check_starting_point = False  # if we didn't check the starting poin，set it to False

        # if we need to chekc the starting point limit, can set it here -> but not really need here (the setting here is 0, 90, 0, 90, 0, 0) which is the HOME position
        self.starting_point_limits = {
            "shoulder_pan_joint": [-0.1, 0.1], # this interval is -0.1 <= x <= 0.1 joint is able to work within this interval
            "shoulder_lift_joint": [-1.6, -1.5],
            "elbow_joint": [-0.1, 0.1],
            "wrist_1_joint": [-1.6, -1.5],
            "wrist_2_joint": [-0.1, 0.1],
            "wrist_3_joint": [-0.1, 0.1],
        }
        
        # 宏義各個目標
        self.declare_parameter("Joints1", [-1.57, -2.07, 0.0, -1.04, 0.0, 0.0])
        self.declare_parameter("Joints2", [-1.57, -1.04, 0.0, -2.35, 0.0, 0.0])
        

        self.goals_dict = {
            "pos1": {"positions": self.get_parameter("Joints1").value},
            "pos2": {"positions": self.get_parameter("Joints2").value},
            # "pos3": {"positions": [0.0, -1.57, 0.0, 0.0, -0.785, 0.0]},
            # "pos4": {"positions": [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]},
        }
        
        # read the target and create JointTrajectoryPoint
        self.goals = []
        for goal in self.goal_name:
            goal_data = self.goals_dict.get(goal, {})
            positions = goal_data.get("positions", [])
            if len(positions) == len(self.joints): # length of position should == how many joints (each joints should have one position)

                point = JointTrajectoryPoint()
                point.positions = positions
                point.time_from_start = Duration(sec=self.wait_sec_between_publish, nanosec=0)
                self.goals.append(point)

                self.get_logger().info(f"Loaded goal {goal}: {positions}")
            else:

                self.get_logger().warn(f"Goal {goal} positions not defined properly.")
         
        if len(self.goals) == 0:
            self.get_logger().error("No valid goal found. Exiting...")
            exit(1)

        # set Publisher，chosse the correct topic that is going to publish
        publish_topic = "/" + self.controller_name + "/" + "joint_trajectory"
        self.get_logger().info(f"Publishing {len(self.goals)} goals on topic '{publish_topic}' every {self.wait_sec_between_publish} second")
        self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 10)
        self.timer = self.create_timer(self.wait_sec_between_publish, self.timer_callback)
        self.current_goal_index = 0

    def timer_callback(self):
        traj = JointTrajectory()
        traj.joint_names = self.joints
        traj.points.append(self.goals[self.current_goal_index])
        self.publisher_.publish(traj)
        self.get_logger().info(f"Published goal: {self.goals[self.current_goal_index].positions}")
        self.current_goal_index = (self.current_goal_index + 1) % len(self.goals)

def main(args=None):
    
    rclpy.init(args=args)
    node = UnifiedTrajectoryPublisher("ur3e_topic")

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("shutting down...")

    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    
    main()