#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import os
import time
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.colors import LinearSegmentedColormap

class FemtoBoltViewer:
    def __init__(self, depth_min=900, depth_max=1300):
        self.depth_min = depth_min
        self.depth_max = depth_max

        # 自定义colormap
        colors = ['fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
                  'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue']
        self.cmap = LinearSegmentedColormap.from_list("custom_cmap", colors)

        # SDK设备句柄
        self.device_handle = None
        self.pykinect = None
        self.config = None

        # 初始化matplotlib figure
        plt.ion()
        self.fig, self.ax = plt.subplots(figsize=(7, 7))

    def _load_sdk(self):
        """加载FemtoBolt SDK"""
        try:
            import pykinect_azure as pykinect
            self.pykinect = pykinect
            # 配置DLL路径
            base_dir = os.path.dirname(os.path.abspath(__file__))
            dll_path = os.path.join(base_dir, "..", "dll", "femtobolt", "bin", "k4a.dll")
            self.pykinect.initialize_libraries(track_body=False, module_k4a_path=dll_path)
            return True
        except Exception as e:
            print(f"加载SDK失败: {e}")
            return False

    def _configure_device(self):
        """配置FemtoBolt设备"""
        self.config = self.pykinect.default_configuration
        self.config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_UNBINNED
        self.config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_15
        self.config.synchronized_images_only = False
        self.config.color_resolution = 0
        self.device_handle = self.pykinect.start_device(config=self.config)

    def _render_depth(self, depth_image: np.ndarray):
        """使用matplotlib绘制深度图，带背景和自定义colormap"""
        # 过滤深度范围
        depth = np.where((depth_image >= self.depth_min) & (depth_image <= self.depth_max),
                         depth_image, 0)

        # 屏蔽深度为0的部分
        depth = np.ma.masked_equal(depth, 0)

        # 背景图（灰色）
        background = np.ones_like(depth) * 0.5

        self.ax.clear()
        # 绘制背景
        self.ax.imshow(background, origin='lower', cmap='gray', alpha=0.3)
        # 绘制白色栅格线
        self.ax.grid(True, which='both', axis='both', color='white', linestyle='-', linewidth=1, zorder=0)
        # 绘制深度等高线
        self.ax.contourf(depth, levels=200, cmap=self.cmap, vmin=self.depth_min, vmax=self.depth_max, origin='upper', zorder=2)
        plt.pause(0.001)
        plt.draw()

    def run(self):
        if not self._load_sdk():
            print("SDK加载失败，退出")
            return

        self._configure_device()
        print("FemtoBolt深度相机启动成功，按 Ctrl+C 退出")

        try:
            while True:
                capture = self.device_handle.update()
                if capture is None:
                    continue

                ret, depth_image = capture.get_depth_image()
                if not ret or depth_image is None:
                    continue

                self._render_depth(depth_image)

        except KeyboardInterrupt:
            print("退出程序")
        finally:
            self.device_handle.stop()
            self.device_handle.close()
            plt.close(self.fig)


if __name__ == "__main__":
    viewer = FemtoBoltViewer(depth_min=900, depth_max=1300)
    viewer.run()