import rclpy
import math
from rclpy.node import Node
from std_msgs.msg import Int16

import tkinter as tk
from tkinter import messagebox
from fanuc_msg_translator_msgs.msg import FanucPayloadNum
from trajectory_msgs.msg import JointTrajectoryPoint


class FanucControlPanelGUI(Node):

    def __init__(self):
        super().__init__('fanuc_control_panel_gui')
        self.publisher_goal_point = self.create_publisher(JointTrajectoryPoint, 'goal_point', 10)
        self.publisher_int = self.create_publisher(FanucPayloadNum, '/fanuc_1/bridge/set_payload_num/goal', 10)
        self.publisher_cancel_trajectory = self.create_publisher(Int16, 'cancel_trajectory', 10)

        # Initialize the Tkinter GUI
        self.root = tk.Tk()
        self.root.title('ROS2 GUI')

        # First segment: 6 input fields and a button
        self.segment1 = tk.Frame(self.root)
        self.segment1.pack(pady=10)

        self.input_fields = []
        for i in range(6):
            label = tk.Label(self.segment1, text=f'Joint {i+1}: ')
            label.grid(row=i, column=0, padx=10, pady=5)

            input_field = tk.Entry(self.segment1)
            input_field.grid(row=i, column=1, padx=10, pady=5)
            self.input_fields.append(input_field)

        button_float = tk.Button(self.segment1, text='Send Joint Angles', command=self.send_goal_pose)
        button_float.grid(row=6, column=0, columnspan=2, pady=5)

        # Second segment: 1 input field and a button
        self.segment2 = tk.Frame(self.root)
        self.segment2.pack(pady=10)

        label_cancel = tk.Label(self.segment2, text='Cancel Trajectory: ')
        label_cancel.grid(row=0, column=0, padx=10, pady=5)

        self.input_field_cancel = tk.Entry(self.segment2)
        self.input_field_cancel.grid(row=0, column=1, padx=10, pady=5)

        button_cancel = tk.Button(self.segment2, text='Cancel', command=self.send_cancel_trajectory)
        button_cancel.grid(row=1, column=0, columnspan=2, pady=5)

        # Second segment: 1 input field and a button
        self.segment3 = tk.Frame(self.root)
        self.segment3.pack(pady=10)

        label_int = tk.Label(self.segment3, text='Payload Nr: ')
        label_int.grid(row=0, column=0, padx=10, pady=5)

        self.input_field_int = tk.Entry(self.segment3)
        self.input_field_int.grid(row=0, column=1, padx=10, pady=5)

        button_int = tk.Button(self.segment3, text='Send Payload Nr', command=self.send_payload_nr)
        button_int.grid(row=1, column=0, columnspan=2, pady=5)

        # Start the ROS2 node
        self.start()

    def send_goal_pose(self):
        try:
            values = [float(field.get())*(2*math.pi)/360 for field in self.input_fields]
            msg = JointTrajectoryPoint()
            msg.positions = values
            self.publisher_goal_point.publish(msg)
            self.get_logger().info(f'Goal position: {values}')
        except ValueError:
            messagebox.showerror('Error', 'Invalid float value')

    def send_payload_nr(self):
        try:
            value = int(self.input_field_int.get())
            fanuc_payload_num = FanucPayloadNum()
            fanuc_payload_num.payload_num = value
            self.publisher_int.publish(fanuc_payload_num)
            self.get_logger().info(f'Payload set to Nr: {value}')
        except ValueError:
            messagebox.showerror('Error', 'Invalid integer value')

    def send_cancel_trajectory(self):
        try:
            value = int(self.input_field_cancel.get())
            msg = Int16()
            msg.data = value
            self.publisher_cancel_trajectory.publish(msg)
            self.get_logger().info(f'Cancel trajectory: {value}')
        except ValueError:
            messagebox.showerror('Error', 'Invalid integer value')

    def start(self):
        self.root.mainloop()


def main(args=None):
    rclpy.init(args=args)
    gui_node = FanucControlPanelGUI()
    rclpy.spin(gui_node)
    gui_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
