"""
IMU 3D 轨迹实时可视化

matplotlib 3D 窗口, 30Hz 刷新, 显示:
  - 蓝色轨迹线
  - 当前点红点
  - 原点坐标系指示
  - 等比例坐标轴
"""

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation


class TrajectoryViewer:
    """3D 轨迹实时显示窗口"""

    def __init__(self, title="IMU 3D Odometry", refresh_interval=33):
        """
        Args:
            title:           窗口标题
            refresh_interval: 刷新间隔 (ms), 默认 33ms ≈ 30Hz
        """
        self.refresh_interval = refresh_interval

        self.fig = plt.figure(figsize=(8, 7))
        self.fig.canvas.manager.set_window_title(title)
        self.ax = self.fig.add_subplot(111, projection='3d')

        # 轨迹线 (蓝色)
        self.traj_line, = self.ax.plot([], [], [], 'b-', linewidth=1.0, label='Trajectory')

        # 当前点 (红色)
        self.current_point, = self.ax.plot([], [], [], 'ro', markersize=6, label='Current')

        # 坐标系指示 (原点处)
        axis_len = 0.3
        self.origin_axes = [
            self.ax.quiver(0, 0, 0, axis_len, 0, 0, color='r', arrow_length_ratio=0.15, label='X'),
            self.ax.quiver(0, 0, 0, 0, axis_len, 0, color='g', arrow_length_ratio=0.15, label='Y'),
            self.ax.quiver(0, 0, 0, 0, 0, axis_len, color='b', arrow_length_ratio=0.15, label='Z'),
        ]

        self._setup_axes()

        # 动画
        self.anim = FuncAnimation(self.fig, self._animate, interval=self.refresh_interval,
                                   cache_frame_data=False, blit=False)

    def _setup_axes(self):
        """初始化坐标轴"""
        self.ax.set_xlabel('X (front)')
        self.ax.set_ylabel('Y (left)')
        self.ax.set_zlabel('Z (up)')
        self.ax.set_title("IMU 3D Trajectory (Z-up)")
        self.ax.legend(loc='upper left')

        # 初始范围
        self.ax.set_xlim([-1, 1])
        self.ax.set_ylim([-1, 1])
        self.ax.set_zlim([-1, 1])

        try:
            self.ax.set_box_aspect([1, 1, 1])
        except NotImplementedError:
            pass

        self.ax.grid(True)

    def _animate(self, frame):
        """动画帧回调 (不做任何事, 数据由外部 update 驱动)"""
        pass  # 通过 plt.pause 驱动, FuncAnimation 仅用于保持窗口响应

    def update(self, history_array):
        """更新显示的轨迹数据

        Args:
            history_array: Nx3 numpy array, 位置历史
        """
        if len(history_array) < 1:
            return

        x, y, z = history_array[:, 0], history_array[:, 1], history_array[:, 2]

        # 更新轨迹线
        self.traj_line.set_data(x, y)
        self.traj_line.set_3d_properties(z)

        # 更新当前点
        self.current_point.set_data([x[-1]], [y[-1]])
        self.current_point.set_3d_properties([z[-1]])

        # 自适应坐标轴范围
        self._auto_scale(x, y, z)

    def _auto_scale(self, x, y, z):
        """根据数据自动调整坐标轴范围, 保持等比例"""
        all_coords = np.concatenate([x, y, z])
        margin = max(np.ptp(all_coords) * 0.2, 0.5)
        mid = (all_coords.min() + all_coords.max()) / 2
        half = (all_coords.max() - all_coords.min()) / 2 + margin

        self.ax.set_xlim([mid - half, mid + half])
        self.ax.set_ylim([mid - half, mid + half])
        self.ax.set_zlim([mid - half, mid + half])

    def show(self):
        """阻塞显示窗口"""
        plt.show()

    def close(self):
        """关闭窗口"""
        plt.close(self.fig)