import tkinter as tk
import rclpy
from rclpy.node import Node
from fanuc_msg_translator_msgs.msg import FanucRobotStatus, FanucRobotStatusTriState


class RobotStatusSubscriber(Node):
    def __init__(self):
        super().__init__('robot_status_subscriber')
        self.subscription = self.create_subscription(
            FanucRobotStatus,
            '/fanuc_1/bridge/robot_status',
            self.robot_status_callback,
            10
        )
        # Create the GUI window
        self.window = tk.Tk()
        self.window.title('Robot Status')
        self.labels = []
        self.create_gui()

        




    def robot_status_callback(self, msg):
        self.get_logger().info("Values: " + str(msg))
        # Extract values from the FanucRobotStatus message
        tp_enabled = msg.tp_enabled.val
        e_stopped = msg.e_stopped.val
        drives_powered = msg.drives_powered.val
        motion_possible = msg.motion_possible.val
        in_motion = msg.in_motion.val
        in_error = msg.in_error.val
        error_code = msg.error_code

        values = [tp_enabled, e_stopped, drives_powered, motion_possible,
                  in_motion, in_error, error_code]

        for label in self.labels:
            label.config(text=str(values[self.labels.index(label)]))
            if self.labels.index(label) < 5:
                label.config(fg='green' if values[self.labels.index(label)] == 1 else 'red')

        self.window.update()









    def create_gui(self):
        tk.Label(self.window, text='TP Enabled').grid(row=0, column=0, sticky='e')
        tk.Label(self.window, text='E-Stopped').grid(row=1, column=0, sticky='e')
        tk.Label(self.window, text='Drives Powered').grid(row=2, column=0, sticky='e')
        tk.Label(self.window, text='Motion Possible').grid(row=3, column=0, sticky='e')
        tk.Label(self.window, text='In Motion').grid(row=4, column=0, sticky='e')
        tk.Label(self.window, text='In Error').grid(row=5, column=0, sticky='e')
        tk.Label(self.window, text='Error Code').grid(row=6, column=0, sticky='e')

        self.labels.append(tk.Label(self.window, text='0'))
        self.labels[0].grid(row=0, column=1, sticky='w')
        self.labels.append(tk.Label(self.window, text='0'))
        self.labels[1].grid(row=1, column=1, sticky='w')
        self.labels.append(tk.Label(self.window, text='0'))
        self.labels[2].grid(row=2, column=1, sticky='w')
        self.labels.append(tk.Label(self.window, text='0'))
        self.labels[3].grid(row=3, column=1, sticky='w')
        self.labels.append(tk.Label(self.window, text='0'))
        self.labels[4].grid(row=4, column=1, sticky='w')
        self.labels.append(tk.Label(self.window, text='0'))
        self.labels[5].grid(row=5, column=1, sticky='w')
        self.labels.append(tk.Label(self.window, text='0'))
        self.labels[6].grid(row=6, column=1, sticky='w')

        self.window.update()






def main(args=None):
    rclpy.init(args=args)
    subscriber = RobotStatusSubscriber()
    rclpy.spin(subscriber)
    subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

