import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

import numpy as np
import time

class RobotNode(Node):

    def __init__(self):
        super().__init__('robot_node')

        self.l1 = 0.5
        self.l2 = 0.3
        self.x = self.l1 + self.l2
        self.y = 0.0
        self.theta1 = 0.0
        self.theta2 = 0.0

        self.publisher = self.create_publisher(
            JointState, 
            '/joint_states', 
            10
        )

        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = JointState()
        msg.name = ['joint1', 'joint2']
        msg.position = [self.theta1, self.theta2]
        self.publisher.publish(msg)
        self.get_logger().info(f'x = {self.x}, y = {self.y}, theta1 = {self.theta1}, theta2 = {self.theta2}')


        self.subcriber = self.create_subscription(
            JointTrajectory,
            '/joint_trajectory_controller/joint_trajectory',
            self.joint_trajectory_callback,
            10
        )

    def joint_trajectory_callback(self, msg):
        joint_names = msg.joint_names
        duration = 0
        for point in msg.points:
            duration = msg.time_from_start_sec + msg.time_from_start_nanosec / 1e9 - duration
            for i in range(duration *10):
                self.theta1 += (point.positions[0] - self.theta1) / 10
                self.theta2 += (point.positions[1] - self.theta2) / 10
                self.x = self.l1 * np.cos(self.theta1) + self.l2 * np.cos(self.theta1 + self.theta2)
                self.y = self.l1 * np.sin(self.theta1) + self.l2 * np.sin(self.theta1 + self.theta2)
                time.sleep(0.1)


def main(args=None):
    rclpy.init(args=args)
    node = RobotNode()
    rclpy.spin(node)
    rclpy.shutdown()