import sys
import numpy as np
import matplotlib

matplotlib.use('Qt5Agg')  # 必须在导入 pyplot 前设置

from PyQt5.QtWidgets import QApplication, QDialog, QVBoxLayout
from PyQt5.QtGui import QStandardItemModel, QStandardItem  # ✅ 正确的模块
from calculate.trajectory import circle_trajectory, line_trajectory
from calculate.ik import inverseF
from matplotlib.figure import Figure
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.animation import FuncAnimation

# 导入你生成的界面
from untitled import Ui_Dialog

import matplotlib.pyplot as plt

# 设置中文字体和解决负号显示问题
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong']  # 按优先级选择字体
plt.rcParams['axes.unicode_minus'] = False  # 显示负号 -

# 杆长参数
L1 = 250
L2 = 300
L3 = 300
L4 = 250
L0 = 250


class MyDialog(QDialog, Ui_Dialog):
    def __init__(self):
        super().__init__()
        self.setupUi(self)

        # ****** 新增：为 listView 初始化模型 ******
        self.angle_model = QStandardItemModel(self.listView)
        self.listView.setModel(self.angle_model)

        # 创建 matplotlib 图形
        self.figure = Figure(figsize=(8, 6), dpi=100)
        self.ax = self.figure.add_subplot(111)
        self.ax.set_xlim(-300, 500)
        self.ax.set_ylim(0, 500)
        self.ax.set_aspect('equal')
        self.ax.grid(True)
        self.ax.set_title("五连杆沿轨迹运动")

        # 创建画布
        self.canvas = FigureCanvas(self.figure)
        self.widget.setLayout(QVBoxLayout())
        self.widget.layout().addWidget(self.canvas)

        # 添加 line 对象
        self.line, = self.ax.plot([], [], 'r-o', linewidth=2, markersize=6)
        # 初始化轨迹数据
        self.x_list = []
        self.y_list = []
        self.ani = None  # 动画对象

        # 连接按钮
        self.pushButton.clicked.connect(self.start_line_trajectory)
        self.pushButton_2.clicked.connect(self.start_circle_trajectory)

    def start_line_trajectory(self):
        # 清除旧的角度数据显示
        self.angle_model.clear()

        self.ax.set_title("五连杆 - 直线轨迹运动")
        x, y = line_trajectory(start=(125, 300), end=(125, 400))
        self.start_animation(x, y)

    def start_circle_trajectory(self):
        # 清除旧的角度数据显示
        self.angle_model.clear()

        self.ax.set_title("五连杆 - 圆形轨迹运动")
        x, y = circle_trajectory(center=(100, 200), radius=40)
        self.start_animation(x, y)

    def start_animation(self, x_list, y_list):
        self.x_list = x_list
        self.y_list = y_list

        # 安全停止旧动画
        try:
            if self.ani is not None and self.ani.event_source is not None:
                self.ani.event_source.stop()
        except:
            pass  # 忽略停止过程中的任何异常


        # 动画函数
        def draw_frame(i):
            x = x_list[i]
            y = y_list[i]
            try:
                theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)

                # ****** 新增：将弧度转换为角度并显示 ******
                theta1_deg = np.degrees(theta1)
                theta4_deg = np.degrees(theta4)
                angle_text = f"点 {i}: θ1={theta1_deg:.1f}°, θ4={theta4_deg:.1f}°"

                # 限制 listView 只显示最近 10 条
                if self.angle_model.rowCount() >= 10:
                    self.angle_model.removeRow(0)
                # 创建新项并添加
                item = QStandardItem(angle_text)
                self.angle_model.appendRow(item)

                # 左侧电机臂末端
                x2 = L1 * np.cos(theta1)
                y2 = L1 * np.sin(theta1)

                # 右侧电机臂末端
                x4 = L4 * np.cos(theta4) + L0
                y4 = L4 * np.sin(theta4)

                # 构建点序列
                x_coords = [0, x2, x, x4, L0]
                y_coords = [0, y2, y, y4, 0]

                self.line.set_data(x_coords, y_coords)
            except Exception as e:
                print(f"第 {i} 帧错误: {e}")
            return self.line,

        # 创建新动画
        self.ani = FuncAnimation(
            self.figure,
            draw_frame,
            frames=len(x_list),
            interval=100,
            repeat=False ,
            blit=False  # PyQt 中 blit=True 有时有问题，先设 False
        )
        self.canvas.draw()


if __name__ == "__main__":
    app = QApplication(sys.argv)
    dialog = MyDialog()
    dialog.show()
    sys.exit(app.exec_())