import rclpy
from fanuc_msg_translator_msgs.msg import FanucTrajectoryConverterQueue, FanucTrajectoryConverterQueueItem, \
    FanucTrajectoryConverterCancel
from rclpy.node import Node
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from std_msgs.msg import Header, Int16


class FanucControlPanelConnector(Node):
    def __init__(self):
        super().__init__("fanuc_control_panel_connector")
        self.currentPosition = JointTrajectoryPoint()
        self.trajectoryPublisher = self.create_publisher(JointTrajectory,
                                                         '/fanuc_1/bridge/follow_joint_trajectory/goal', 10)
        self.trajectoryPublisher2 = self.create_publisher(JointTrajectory, '/fanuc_2/bridge/follow_joint_trajectory/goal', 10)
        self.jointStateSubscription = self.create_subscription(JointState, '/fanuc_1/joint_states',
                                                               self.joint_states_callback,
                                                               10)
        self.goalSubscription = self.create_subscription(JointTrajectoryPoint, 'goal_point', self.goal_points_callback,
                                                         10)
        self.goalListSubscription = self.create_subscription(JointTrajectory, 'goal_point_list',
                                                             self.goal_points_list_callback,
                                                             10)
        self.cancelTrajectorySubscription = self.create_subscription(Int16, 'cancel_trajectory',
                                                                     self.cancel_trajectory_callback, 10)

        self.queueSubscription = self.create_subscription(FanucTrajectoryConverterQueue, '/fanuc_1/bridge'
                                                                                         '/follow_joint_trajectory'
                                                                                         '/queue',
                                                          self.queue_callback, 10)
        self.queue = FanucTrajectoryConverterQueue()

        self.cancelPublisher = self.create_publisher(FanucTrajectoryConverterCancel, '/fanuc_1/bridge/follow_joint_trajectory/cancel', 10)

    def send_trajectory(self, trajectory):
        self.trajectoryPublisher.publish(trajectory)
        self.trajectoryPublisher2.publish(trajectory)
        self.get_logger().info('Published JointTrajectory message')

    def joint_states_callback(self, joint_states):
        self.currentPosition.positions = joint_states.position

    def goal_points_callback(self, goal_points):
        goal_points.time_from_start.sec = 1
        trajectory = JointTrajectory()
        trajectory.header.frame_id = 'base_link'
        trajectory.header.stamp = self.get_clock().now().to_msg()
        trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6']
        trajectory.points.append(self.currentPosition)
        trajectory.points.append(goal_points)

        self.send_trajectory(trajectory)

    def goal_points_list_callback(self, goal_point_list):
        trajectory = JointTrajectory()
        trajectory.header = Header()
        trajectory.header.frame_id = 'base_link'
        trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6']

        trajectory.points = (goal_point_list.points)
        trajectory.points.insert(0, self.currentPosition)
        self.send_trajectory(trajectory)

    def cancel_trajectory_callback(self, cancel_trajectory):
        try:
            cancel = FanucTrajectoryConverterCancel()

            self.get_logger().info('Canceling trajectory with index {}'.format(cancel_trajectory.data))
            self.get_logger().info('Queue ' + str(self.queue.goals))
            queue_item = self.queue.goals[cancel_trajectory.data]

            cancel.received_time_stamp = queue_item.received_time_stamp

            self.cancelPublisher.publish(cancel)
        except IndexError:
            self.get_logger().info('IndexError: No item with index {} in queue'.format(cancel_trajectory.data))

    def queue_callback(self, queue):
        self.queue = queue


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


if __name__ == '__main__':
    main()
