import sys
import math
from PyQt5.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, 
                             QHBoxLayout, QPushButton, QLabel, QGroupBox, 
                             QTextEdit, QLineEdit, QCheckBox, QStackedWidget, 
                             QListWidget, QListWidgetItem, QProgressBar, QComboBox, QGridLayout, QFormLayout)
from PyQt5.QtCore import Qt, QTimer, QSize, QPointF
from PyQt5.QtGui import QFont, QTextCursor, QPainter, QColor, QPen, QBrush, QPolygonF

# =================================================================
# 🗺️ 共享组件：车间 2D 数字孪生沙盘 (高对比白底版)
# =================================================================
class WorkshopMapWidget(QWidget):
    def __init__(self):
        super().__init__()
        self.setMinimumSize(400, 300)
        self.setStyleSheet("background-color: #FFFFFF; border: 1px solid #D1D5DB; border-radius: 6px;")
        self.agv_x, self.agv_y, self.agv_yaw = 0.0, 0.0, 0.0 
        self.scale = 35.0

    def update_pose(self, x, y, yaw):
        self.agv_x, self.agv_y, self.agv_yaw = x, y, yaw
        self.update()

    def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)
        w, h = self.width(), self.height()
        cx, cy = w / 2, h / 2

        # 浅灰网格
        painter.setPen(QPen(QColor("#E5E7EB"), 1, Qt.DashLine))
        for i in range(-20, 21):
            px = cx + i * self.scale
            painter.drawLine(int(px), 0, int(px), h)
            py = cy - i * self.scale
            painter.drawLine(0, int(py), w, int(py))

        # 绝对坐标系原点
        painter.setPen(QPen(QColor(220, 38, 38, 200), 2))
        painter.drawLine(int(cx), int(cy), int(cx + 50), int(cy))
        painter.setPen(QPen(QColor(22, 163, 74, 200), 2))
        painter.drawLine(int(cx), int(cy), int(cx), int(cy - 50))

        # 绘制 AGV (宝蓝色高亮)
        pixel_x = cx + (self.agv_x * self.scale)
        pixel_y = cy - (self.agv_y * self.scale) 
        painter.translate(pixel_x, pixel_y)
        painter.rotate(-math.degrees(self.agv_yaw))

        car_l, car_w = 1.2 * self.scale, 0.7 * self.scale  
        painter.setBrush(QBrush(QColor(37, 99, 235, 80)))
        painter.setPen(QPen(QColor(37, 99, 235), 2))
        
        poly = QPolygonF([QPointF(car_l/2, 0), QPointF(-car_l/2, -car_w/2), QPointF(-car_l/4, 0), QPointF(-car_l/2, car_w/2)])
        painter.drawPolygon(poly)

# =================================================================
# 🖥️ 主控台：侧边栏 + 多页面栈架构
# =================================================================
class CalibrationDashboard(QMainWindow):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("🚀 L4 AGV 自动化车间调度总控台 (v3.0 工业侧边栏版)")
        self.resize(1350, 850)
        
        # 🚨 全局样式：深色侧边栏 + 明亮工作区
        self.setStyleSheet("""
            QMainWindow { background-color: #F3F4F6; color: #1F2937; font-family: 'Microsoft YaHei', sans-serif; }
            
            /* 侧边栏样式 */
            QListWidget { background-color: #1E293B; color: #9CA3AF; border: none; font-size: 15px; font-weight: bold; outline: 0; padding-top: 10px;}
            QListWidget::item { padding: 15px 20px; border-bottom: 1px solid #334155; }
            QListWidget::item:hover { background-color: #334155; color: #F8FAFC; }
            QListWidget::item:selected { background-color: #2563EB; color: #FFFFFF; border-left: 4px solid #60A5FA; }
            
            /* 业务面板样式 */
            QGroupBox { font-weight: bold; color: #1D4ED8; font-size: 14px; border: 1px solid #D1D5DB; border-radius: 6px; margin-top: 15px; background-color: #FFFFFF; }
            QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 5px; color: #1D4ED8;}
            QLabel { color: #1F2937; font-weight: bold; background: transparent; }
            QLineEdit, QComboBox { background-color: #F9FAFB; border: 1px solid #D1D5DB; padding: 6px; border-radius: 4px; font-weight: bold;}
            QPushButton { font-weight: bold; font-size: 14px; border-radius: 5px; padding: 10px; border: none; background-color: #E5E7EB; color: #1F2937;}
            QPushButton:hover { background-color: #D1D5DB; }
            QPushButton:disabled { background-color: #E5E7EB; color: #9CA3AF; } 
            QCheckBox { font-weight: bold; font-size: 13px; }
        """)

        self.sim_time = 0.0
        self.is_connected = False
        
        self.init_ui()
        
        # 模拟外部真值发送高频坐标
        self.sim_timer = QTimer(self)
        self.sim_timer.timeout.connect(self.simulate_ground_truth)

    def init_ui(self):
        main_widget = QWidget()
        self.setCentralWidget(main_widget)
        main_layout = QHBoxLayout(main_widget)
        main_layout.setContentsMargins(0, 0, 0, 0)
        main_layout.setSpacing(0)

        # ==========================================
        # 🗂️ 1. 左侧导航栏 (Sidebar)
        # ==========================================
        sidebar_container = QWidget()
        sidebar_container.setFixedWidth(240)
        sidebar_container.setStyleSheet("background-color: #1E293B;")
        sidebar_layout = QVBoxLayout(sidebar_container)
        sidebar_layout.setContentsMargins(0, 0, 0, 0)

        # 侧边栏顶部 Logo
        lbl_logo = QLabel("🚀 AGV Calib Studio")
        lbl_logo.setStyleSheet("color: #60A5FA; font-size: 18px; font-weight: bold; padding: 20px 10px; background-color: #0F172A;")
        lbl_logo.setAlignment(Qt.AlignCenter)
        sidebar_layout.addWidget(lbl_logo)

        self.nav_list = QListWidget()
        nav_items = [
            "🏠 首页: 车辆会话与画像", 
            "⚙️ 阶段一: 底盘能力标定", 
            "🧠 阶段二: 运控参数寻优", 
            "📷 阶段三: 传感器走停拍"
        ]
        for text in nav_items:
            item = QListWidgetItem(text)
            self.nav_list.addItem(item)
            
        self.nav_list.currentRowChanged.connect(self.switch_page)
        sidebar_layout.addWidget(self.nav_list)

        # ==========================================
        # 📑 2. 右侧工作区 (上: 堆叠页面, 下: 全局日志)
        # ==========================================
        right_container = QWidget()
        right_layout = QVBoxLayout(right_container)
        right_layout.setContentsMargins(15, 15, 15, 15)

        self.stacked_widget = QStackedWidget()
        
        # 实例化四个功能页面
        self.page_home = self.create_page_home()
        self.page_chassis = self.create_page_chassis()
        self.page_control = self.create_page_control()
        self.page_sensor = self.create_page_sensor()
        
        self.stacked_widget.addWidget(self.page_home)
        self.stacked_widget.addWidget(self.page_chassis)
        self.stacked_widget.addWidget(self.page_control)
        self.stacked_widget.addWidget(self.page_sensor)

        # 💻 底部全局系统日志 (无论切到哪页，日志永远可见！)
        grp_log = QGroupBox("💻 全局调度中心日志 (Session Console)")
        lyt_log = QVBoxLayout(grp_log)
        self.txt_log = QTextEdit()
        self.txt_log.setReadOnly(True)
        self.txt_log.setFont(QFont("Consolas", 11, QFont.Bold))
        self.txt_log.setStyleSheet("background-color: #F8FAFC; color: #15803D; border: 1px solid #D1D5DB; padding: 5px;")
        self.txt_log.setFixedHeight(150)
        lyt_log.addWidget(self.txt_log)

        right_layout.addWidget(self.stacked_widget, 1)
        right_layout.addWidget(grp_log)

        main_layout.addWidget(sidebar_container)
        main_layout.addWidget(right_container)
        
        self.nav_list.setCurrentRow(0)
        self.append_log("✅ 自动化车间多页签 UI 挂载成功！")
        self.append_log("📌 请在 [首页] 完成 Session 握手，并下发 Vehicle Profile。")

    # ---------------------------------------------------------
    # 📄 页面 1: 首页 (对应 vehicle_profile.proto)
    # ---------------------------------------------------------
    def create_page_home(self):
        page = QWidget()
        layout = QHBoxLayout(page)
        
        left_lyt = QVBoxLayout()
        grp_net = QGroupBox("🌐 1. 车端节点寻址与心跳")
        lyt_net = QFormLayout(grp_net)
        self.in_ip = QLineEdit("192.168.31.105")
        lyt_net.addRow("车端 Agent IP:", self.in_ip)
        btn_conn = QPushButton("📡 发送 HeartbeatRequest")
        btn_conn.setStyleSheet("background-color: #16A34A; color: white;")
        btn_conn.clicked.connect(self.mock_connect)
        lyt_net.addRow("", btn_conn)
        
        grp_prof = QGroupBox("📋 2. 车辆画像配置 (Vehicle Profile)")
        lyt_prof = QFormLayout(grp_prof)
        lyt_prof.addRow("Session ID:", QLineEdit("SES-20260308-01"))
        lyt_prof.addRow("车辆唯一 ID:", QLineEdit("AGV-T01-Pro"))
        
        cb_chassis = QComboBox()
        cb_chassis.addItems(["DIFFERENTIAL (差速)", "ACKERMANN (阿克曼)", "MULTI_STEER (多舵轮)"])
        lyt_prof.addRow("底盘构型:", cb_chassis)
        
        cb_algo = QComboBox()
        cb_algo.addItems(["PID + 纯追踪 (PP)", "MPC", "LQR"])
        lyt_prof.addRow("运控算法:", cb_algo)
        
        btn_recipe = QPushButton("📝 提交画像生成 Recipe 任务树")
        btn_recipe.setStyleSheet("background-color: #2563EB; color: white; padding: 15px;")
        btn_recipe.clicked.connect(lambda: self.append_log("✅ 车辆画像下发成功！标定配方 (Recipe) 已激活，请按序流转。"))
        
        left_lyt.addWidget(grp_net)
        left_lyt.addWidget(grp_prof)
        left_lyt.addWidget(btn_recipe)
        left_lyt.addStretch()

        right_lyt = QVBoxLayout()
        grp_map = QGroupBox("📡 外部真值绝对地图 (Ground Truth)")
        lyt_map = QVBoxLayout(grp_map)
        self.lbl_pose_home = QLabel("📍 绝对位姿 -> X: 0.00 | Y: 0.00 | Yaw: 0°")
        self.lbl_pose_home.setStyleSheet("color: #0369A1; font-size: 14px;")
        self.map_widget_home = WorkshopMapWidget()
        lyt_map.addWidget(self.lbl_pose_home)
        lyt_map.addWidget(self.map_widget_home)
        right_lyt.addWidget(grp_map)
        
        layout.addLayout(left_lyt, 1)
        layout.addLayout(right_lyt, 1)
        return page

    # ---------------------------------------------------------
    # 📄 页面 2: 底盘标定 (对应 agv_calib_chassis.proto)
    # ---------------------------------------------------------
    def create_page_chassis(self):
        page = QWidget()
        layout = QHBoxLayout(page)
        
        left_lyt = QVBoxLayout()
        grp_act = QGroupBox("⚙️ 动作原语长任务 (StartMotionPrimitive)")
        lyt_act = QVBoxLayout(grp_act)
        
        actions = ["⬆️ 测距: 直线行驶 5 米", "🔄 测角: 原地自转 360°", "〰️ 测死区: 舵角扫频测试"]
        for act in actions:
            b = QPushButton(act)
            b.setStyleSheet("background-color: #4F46E5; color: white;")
            b.clicked.connect(lambda checked, t=act: self.append_log(f"🚀 提交底盘长任务: {t}。获得 JobID: JOB-CH-991"))
            lyt_act.addWidget(b)
            
        lyt_act.addStretch()
        btn_estop = QPushButton("🛑 触发系统级急停 (Emergency Brake)")
        btn_estop.setStyleSheet("background-color: #DC2626; color: white; padding: 15px;")
        lyt_act.addWidget(btn_estop)
        left_lyt.addWidget(grp_act)
        
        right_lyt = QVBoxLayout()
        grp_tel = QGroupBox("🌊 底盘 50Hz 遥测流 (StreamChassisTelemetry)")
        lyt_tel = QVBoxLayout(grp_tel)
        self.lbl_fl = QLabel("FL 模块: RPM: 0.0 | 脉冲: 0")
        self.lbl_fr = QLabel("FR 模块: RPM: 0.0 | 脉冲: 0")
        for lbl in [self.lbl_fl, self.lbl_fr]:
            lbl.setFont(QFont("Consolas", 14, QFont.Bold))
            lyt_tel.addWidget(lbl)
            
        self.map_widget_chas = WorkshopMapWidget()
        lyt_tel.addWidget(self.map_widget_chas)
        right_lyt.addWidget(grp_tel)
        
        layout.addLayout(left_lyt, 1)
        layout.addLayout(right_lyt, 1)
        return page

    # ---------------------------------------------------------
    # 📄 页面 3: 运控寻优 (对应 agv_calib_control.proto)
    # ---------------------------------------------------------
    def create_page_control(self):
        page = QWidget()
        layout = QHBoxLayout(page)
        
        left_lyt = QVBoxLayout()
        grp_inj = QGroupBox("🧠 控制参数热注入 (InjectControllerParameters)")
        lyt_inj = QFormLayout(grp_inj)
        lyt_inj.addRow("横向 Kp:", QLineEdit("1.5"))
        lyt_inj.addRow("横向 Ki:", QLineEdit("0.0"))
        lyt_inj.addRow("前瞻距离 (Ld):", QLineEdit("1.2"))
        btn_inj = QPushButton("💉 热注入车端内存")
        btn_inj.setStyleSheet("background-color: #D97706; color: white;")
        btn_inj.clicked.connect(lambda: self.append_log("💉 参数已注入车端，当前版本: Ver-1.0.2"))
        lyt_inj.addRow(btn_inj)
        
        grp_eval = QGroupBox("🏎️ 闭环考题评估 (StartControllerEvaluation)")
        lyt_eval = QVBoxLayout(grp_eval)
        btn_eval = QPushButton("🚗 跑 S 型测试轨迹测 RMSE")
        btn_eval.setStyleSheet("background-color: #059669; color: white;")
        btn_eval.clicked.connect(lambda: self.append_log("🚗 轨迹跟踪 Job 已创建，正在收集 ControlTelemetry..."))
        lyt_eval.addWidget(btn_eval)
        
        left_lyt.addWidget(grp_inj)
        left_lyt.addWidget(grp_eval)
        left_lyt.addStretch()

        right_lyt = QVBoxLayout()
        grp_map = QGroupBox("📉 实时闭环循迹监控")
        lyt_map = QVBoxLayout(grp_map)
        self.map_widget_ctrl = WorkshopMapWidget()
        lyt_map.addWidget(self.map_widget_ctrl)
        right_lyt.addWidget(grp_map)
        
        layout.addLayout(left_lyt, 1)
        layout.addLayout(right_lyt, 1)
        return page

    # ---------------------------------------------------------
    # 📄 页面 4: 传感器标定 (对应 agv_calib_sensor.proto)
    # ---------------------------------------------------------
    def create_page_sensor(self):
        page = QWidget()
        layout = QVBoxLayout(page)
        
        grp_cap = QGroupBox("📷 走停拍与同步锁存 (SynchronizedCapture)")
        lyt_cap = QVBoxLayout(grp_cap)
        
        h_checks = QHBoxLayout()
        h_checks.addWidget(QCheckBox("前视相机 (cam_front)"))
        h_checks.addWidget(QCheckBox("机械臂相机 (arm_cam)"))
        h_checks.addWidget(QCheckBox("3D激光雷达 (lidar_top)"))
        lyt_cap.addLayout(h_checks)
        
        h_btns = QHBoxLayout()
        btn_move = QPushButton("🚙 1. 移动至底盘/机械臂标定位")
        btn_move.setStyleSheet("background-color: #2563EB; color: white;")
        btn_move.clicked.connect(lambda: self.append_log("🚙 调度 Job 已发: 底盘与机械臂正在前往联合观测点..."))
        
        btn_trig = QPushButton("📸 2. 发起硬同步锁存")
        btn_trig.setStyleSheet("background-color: #DB2777; color: white;")
        btn_trig.clicked.connect(lambda: self.append_log("📸 锁存成功！CaptureID: CAP-778899"))
        h_btns.addWidget(btn_move)
        h_btns.addWidget(btn_trig)
        lyt_cap.addLayout(h_btns)
        
        grp_down = QGroupBox("📥 流式文件切块下载 (DownloadCapturedArtifact)")
        lyt_down = QVBoxLayout(grp_down)
        lyt_down.addWidget(QLabel("Job 状态：轮询拉取 LiDAR 压缩包..."))
        self.prog_bar = QProgressBar()
        self.prog_bar.setValue(0)
        btn_dl = QPushButton("⬇️ 根据 CaptureID 启动文件流传输")
        btn_dl.setStyleSheet("background-color: #7C3AED; color: white;")
        btn_dl.clicked.connect(self.mock_download)
        lyt_down.addWidget(self.prog_bar)
        lyt_down.addWidget(btn_dl)

        layout.addWidget(grp_cap)
        layout.addWidget(grp_down)
        layout.addStretch()
        return page

    # =========================================================
    # 🔄 UI 交互与动画逻辑
    # =========================================================
    def switch_page(self, index):
        self.stacked_widget.setCurrentIndex(index)
        page_names = ["[首页: 画像配置]", "[P1: 底盘运动学]", "[P2: 运控闭环]", "[P3: 传感器外参]"]
        self.append_log(f"👁️ UI 视图已切换至: {page_names[index]}")

    def mock_connect(self):
        if not self.is_connected:
            self.append_log(f"🔄 发送 HeartbeatRequest -> {self.in_ip.text()} ...")
            self.append_log("✅ 收到 HeartbeatResponse: 车辆正常，vehicle_ready = True")
            self.is_connected = True
            self.sim_timer.start(33) 
        else:
            self.append_log("⚠️ 会话连接已物理断开。")
            self.is_connected = False
            self.sim_timer.stop()

    def mock_download(self):
        self.append_log("📥 创建 LRO 异步下载任务，后台轮询 JobStatus...")
        self.prog_bar.setValue(0)
        self.dl_timer = QTimer()
        self.dl_timer.timeout.connect(self._step_dl)
        self.dl_timer.start(50)
        
    def _step_dl(self):
        val = self.prog_bar.value() + 4
        self.prog_bar.setValue(val)
        if val >= 100:
            self.dl_timer.stop()
            self.append_log("✅ JobStatus = SUCCEEDED, 数据文件安全落盘，Digest校验通过！")

    def append_log(self, text):
        self.txt_log.append(text)
        self.txt_log.moveCursor(QTextCursor.End)

    def simulate_ground_truth(self):
        # 假真值发生器：让小车跑一个平滑的 8 字形
        self.sim_time += 0.05
        x = 4.0 * math.sin(self.sim_time * 0.5)
        y = 2.0 * math.sin(self.sim_time)
        dx, dy = 2.0 * math.cos(self.sim_time * 0.5), 2.0 * math.cos(self.sim_time)
        yaw = math.atan2(dy, dx)
        
        pose_str = f"📍 绝对位姿 -> X: {x:+.2f}m | Y: {y:+.2f}m | Yaw: {math.degrees(yaw):+05.1f}°"
        self.lbl_pose_home.setText(pose_str)
        
        # 🚨 魔法：同步更新所有页面的地图实例！
        self.map_widget_home.update_pose(x, y, yaw)
        self.map_widget_chas.update_pose(x, y, yaw)
        self.map_widget_ctrl.update_pose(x, y, yaw)
        
        # 模拟底盘页面的高频遥测数字
        if self.stacked_widget.currentIndex() == 1:
            rpm = abs(80 * math.cos(self.sim_time))
            self.lbl_fl.setText(f"FL 模块: RPM: {rpm:.1f} | 脉冲: {int(self.sim_time*1000)}")
            self.lbl_fr.setText(f"FR 模块: RPM: {rpm:.1f} | 脉冲: {int(self.sim_time*1000)}")

if __name__ == '__main__':
    app = QApplication(sys.argv)
    window = CalibrationDashboard()
    window.show()
    sys.exit(app.exec_())