# -*- coding: UTF-8 -*-
# 6-Axis Independent Servo Controller (Calibrated UI)
# 对应 Arduino: 独立舵机模式 (ID 1-6)
# 初始显示已同步硬件参数
import tkinter as tk
from tkinter import ttk
import serial
import serial.tools.list_ports
import time

class RobotGUI:
    def __init__(self, root):
        self.root = root
        self.root.title("6-Axis Calibration Tool")
        self.root.geometry("460x680") 
        
        # --- 变量初始化 ---
        self.ser = None
        self.step_size = tk.IntVar(value=1) # 默认步进 1度
        
        # [关键] 界面初始数值同步您的黄金参数
        self.angles = {
            1: 90, # Pin 41
            2: 90, # Pin 43 (2-1)
            3: 85, # Pin 45 (2-2) -> 85
            4: 94, # Pin 47 (3-1) -> 94
            5: 90, # Pin 49 (3-2)
            6: 90  # Pin 51 (4)
        }
        self.labels = {} 

        # --- 构建界面 ---
        self.create_connection_area()
        self.create_step_selector()
        self.create_servo_controls()
        self.create_footer()

    def create_connection_area(self):
        frame = tk.LabelFrame(self.root, text="Connection", padx=10, pady=10)
        frame.pack(fill="x", padx=10, pady=5)

        tk.Label(frame, text="Port:").pack(side="left")
        self.port_combo = ttk.Combobox(frame, width=15)
        self.port_combo.pack(side="left", padx=5)
        
        tk.Button(frame, text="Refresh", command=self.refresh_ports).pack(side="left")
        self.btn_connect = tk.Button(frame, text="Connect", command=self.toggle_connection, bg="#DDDDDD")
        self.btn_connect.pack(side="left", padx=5)
        
        self.refresh_ports()

    def create_step_selector(self):
        frame = tk.LabelFrame(self.root, text="Step Size (Degree)", padx=10, pady=10)
        frame.pack(fill="x", padx=10, pady=5)
        
        tk.Radiobutton(frame, text="1°", variable=self.step_size, value=1).pack(side="left", padx=20)
        tk.Radiobutton(frame, text="5°", variable=self.step_size, value=5).pack(side="left", padx=20)
        tk.Radiobutton(frame, text="10°", variable=self.step_size, value=10).pack(side="left", padx=20)

    def create_servo_controls(self):
        frame = tk.LabelFrame(self.root, text="Individual Servo Calibration", padx=10, pady=10)
        frame.pack(fill="both", expand=True, padx=10, pady=5)

        # 生成 6 行控制条 (显示具体名称和引脚)
        self.create_servo_row(frame, 1, "Base (Pin 41)")
        self.create_servo_row(frame, 2, "Big Arm 2-1 (Pin 43)")
        self.create_servo_row(frame, 3, "Big Arm 2-2 (Pin 45)")
        self.create_servo_row(frame, 4, "Mid Arm 3-1 (Pin 47)")
        self.create_servo_row(frame, 5, "Mid Arm 3-2 (Pin 49)")
        self.create_servo_row(frame, 6, "Gripper (Pin 51)")

    def create_servo_row(self, parent, servo_id, name):
        row = tk.Frame(parent)
        row.pack(fill="x", pady=8)

        # 名字
        tk.Label(row, text=name, width=20, anchor="w", font=("Arial", 9)).pack(side="left")
        
        # 减号按钮
        tk.Button(row, text="  <  ", command=lambda: self.move_servo(servo_id, -1)).pack(side="left")
        
        # 角度数值显示
        lbl = tk.Label(row, text=str(self.angles[servo_id]), width=6, font=("Arial", 14, "bold"), fg="blue")
        lbl.pack(side="left", padx=15)
        self.labels[servo_id] = lbl
        
        # 加号按钮
        tk.Button(row, text="  >  ", command=lambda: self.move_servo(servo_id, 1)).pack(side="left")

    def create_footer(self):
        # 归位按钮 (归位到黄金初始值)
        tk.Button(self.root, text="RESET TO CALIBRATION (Safe)", command=self.go_home, bg="#FFCCCC", height=2, font=("Arial", 10, "bold")).pack(fill="x", padx=10, pady=15)

    # --- 逻辑功能 ---

    def refresh_ports(self):
        ports = [p.device for p in serial.tools.list_ports.comports()]
        self.port_combo['values'] = ports
        if ports: self.port_combo.current(0)

    def toggle_connection(self):
        if self.ser and self.ser.is_open:
            self.ser.close()
            self.btn_connect.config(text="Connect", bg="#DDDDDD")
        else:
            try:
                port = self.port_combo.get()
                self.ser = serial.Serial(port, 9600, timeout=1)
                self.btn_connect.config(text="Connected", bg="#99FF99")
                time.sleep(1.5) # 等待Arduino重启
            except Exception as e:
                print(f"Error: {e}")

    def move_servo(self, servo_id, direction):
        step = self.step_size.get()
        current = self.angles[servo_id]
        new_angle = current + (step * direction)

        if new_angle < 0: new_angle = 0
        if new_angle > 180: new_angle = 180

        # 更新数据和界面
        self.angles[servo_id] = new_angle
        self.labels[servo_id].config(text=str(new_angle))
        
        # 发送指令
        self.send_command(servo_id, new_angle)

    def go_home(self):
        # 重置为黄金参数，而不是全部90
        defaults = {1:90, 2:90, 3:85, 4:94, 5:90, 6:90}
        
        for i in range(1, 7):
            safe_angle = defaults[i]
            self.angles[i] = safe_angle
            self.labels[i].config(text=str(safe_angle))
            self.send_command(i, safe_angle)
            time.sleep(0.05)

    def send_command(self, sid, angle):
        if self.ser and self.ser.is_open:
            cmd = f"{sid}:{angle}\n"
            self.ser.write(cmd.encode())
            print(f"Sent: {cmd.strip()}")
        else:
            print(f"Simulation: Servo {sid} -> {angle}")

# --- 启动程序 ---
if __name__ == "__main__":
    root = tk.Tk()
    app = RobotGUI(root)
    root.mainloop()