import rclpy
from rclpy.node import Node
from fanuc_msg_translator_msgs.msg import FanucTrajectoryConverterQueue, FanucTrajectoryConverterQueueItem
import tkinter as tk
from tkinter import ttk


class FanucGUI(Node):

    def __init__(self):
        super().__init__('fanuc_gui')
        self.subscription = self.create_subscription(
            FanucTrajectoryConverterQueue,
            '/fanuc_1/bridge/follow_joint_trajectory/queue',
            self.queue_callback,
            10
        )


        self.root = tk.Tk()
        self.treeview = ttk.Treeview(self.root)
        self.treeview['columns'] = ('Nr', 'time_stamp_s', 'time_stamp_ns', 'status_in_queue', 'status_on_server')

        self.treeview.column('#0', width=0, stretch=tk.NO)
        self.treeview.column('Nr', anchor=tk.CENTER, width=50)
        self.treeview.column('time_stamp_s', anchor=tk.CENTER, width=150)
        self.treeview.column('time_stamp_ns', anchor=tk.CENTER, width=150)
        self.treeview.column('status_in_queue', anchor=tk.CENTER, width=150)
        self.treeview.column('status_on_server', anchor=tk.CENTER, width=150)

        self.treeview.heading('#0', text='', anchor=tk.CENTER)
        self.treeview.heading('Nr', text='Nr', anchor=tk.CENTER)
        self.treeview.heading('time_stamp_s', text='Time Stamp [s]', anchor=tk.CENTER)
        self.treeview.heading('time_stamp_ns', text='Time Stamp [ns]', anchor=tk.CENTER)
        self.treeview.heading('status_in_queue', text='Status in Queue', anchor=tk.CENTER)
        self.treeview.heading('status_on_server', text='Status on Server', anchor=tk.CENTER)

        self.treeview.pack()

    def queue_callback(self, msg):
        self.treeview.delete(*self.treeview.get_children())  # Clear the table

        for item in msg.goals:
            nr = msg.goals.index(item)
            time_stamp_s = item.received_time_stamp.sec
            time_stamp_ns = item.received_time_stamp.nanosec
            status_in_queue = item.status_in_queue
            status_on_server = item.status_on_server

            self.treeview.insert('', tk.END, text='', values=(nr, time_stamp_s, time_stamp_ns, status_in_queue, status_on_server))
        self.root.update()  # Update the GUI



def main(args=None):
    rclpy.init(args=args)
    fanuc_gui = FanucGUI()
    rclpy.spin(fanuc_gui)
    fanuc_gui.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
