# -- coding: utf-8 --
import threading
import msvcrt
import ctypes
import time
import os
import struct
from ctypes import *
from datetime import datetime

from Mv3dRgbdImport.Mv3dRgbdDefine import *
from Mv3dRgbdImport.Mv3dRgbdApi import *
from Mv3dRgbdImport.Mv3dRgbdDefine import DeviceType_Ethernet, DeviceType_USB, DeviceType_Ethernet_Vir, DeviceType_USB_Vir, MV3D_RGBD_FLOAT_EXPOSURETIME, \
    ParamType_Float, ParamType_Int, ParamType_Enum, CoordinateType_Depth, MV3D_RGBD_FLOAT_Z_UNIT, MV3D_RGBD_OK, \
    FileType_BMP,FileType_TIFF,ImageType_Depth, ImageType_RGB8_Planar, ImageType_YUV420SP_NV12  ,ImageType_YUV420SP_NV21 , ImageType_YUV422, ImageType_Mono8

import config as configMap

# 全局变量
SN_MAP = {}  # {sn: camera_instance}


def initialize_devices():
    nDeviceNum = ctypes.c_uint(0)
    ret = Mv3dRgbd.MV3D_RGBD_GetDeviceNumber(
        DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir,
        byref(nDeviceNum)
    )
    if ret != MV3D_RGBD_OK or nDeviceNum.value == 0:
        print("Failed to get device number or no devices found.")
        return

    stDeviceList = MV3D_RGBD_DEVICE_INFO_LIST()
    Mv3dRgbd.MV3D_RGBD_GetDeviceList(
        DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir,
        pointer(stDeviceList.DeviceInfo[0]), 20, byref(nDeviceNum)
    )

    for i in range(nDeviceNum.value):
        serial_number = ''.join(chr(c) for c in stDeviceList.DeviceInfo[i].chSerialNumber).rstrip('\x00')
        print(f"Found device [{i}]: Serial Number: {serial_number}")

        camera = Mv3dRgbd()

        # 打开设备
        ret = camera.MV3D_RGBD_OpenDevice(pointer(stDeviceList.DeviceInfo[i]))
        if ret != MV3D_RGBD_OK:
            print(f"Failed to open device with SN: {serial_number}. Error code: {ret:#x}")
            continue

        # 存入全局 map
        SN_MAP[serial_number] = camera
        print(f"Successfully added device {serial_number} to SN_MAP")


def pic(sn):
    camera = SN_MAP.get(sn)

    if not camera:
        print(f"No camera found for SN: {sn}")
        return

    config = configMap.CAMERA_CONFIG_MAP.get(sn)
    if not config:
        print(f"No config found for SN: {sn}")
        return

    time_on = config.get("time_on", 0)     # 延时开始（毫秒）
    time_off = config.get("time_off", 0)   # 拍照总时长（毫秒）
    time_hop = config.get("time_hop", 0)   # 每次拍照间隔（毫秒）

    end_time = time.time() + (time_off / 1000.0)
    print(f"Delaying start by {time_on}ms...")
    time.sleep(time_on / 1000.0)  # 转成秒


    frame_count = 0

    saved_tiff_files = []  # 用于存储保存的 TIFF 文件路径
    print(f"Start continuous capturing for {time_off}ms...")

    # 开始取流
    ret = camera.MV3D_RGBD_Start()
    if ret != MV3D_RGBD_OK:
        print(f"Failed to start grabbing. Error code: {ret:#x}")
        return

    try:
        while time.time() < end_time:
            stFrameData = MV3D_RGBD_FRAME_DATA()

            # 获取帧数据
            ret = camera.MV3D_RGBD_FetchFrame(pointer(stFrameData), 5000)
            if ret == MV3D_RGBD_OK:
                frame_count += 1

                for i in range(stFrameData.nImageCount):
                    image_info = stFrameData.stImageData[i]

                    # 保存深度图
                    if image_info.enImageType == ImageType_Depth:
                        file_name = configMap.save_path("depth","-Depth")
                        ret_save = camera.MV3D_RGBD_SaveImage(pointer(image_info), FileType_TIFF, file_name)
                        print("Saved depth image." if ret_save == MV3D_RGBD_OK else "Failed to save depth image.")
                        if ret_save == MV3D_RGBD_OK:
                            saved_tiff_files.append(file_name)  # 记录保存的 TIFF 文件路径
                            # print(f"Saved depth image: {file_name}")
                        else:
                            print("Failed to save depth image.")

                    # 保存彩色图
                    elif image_info.enImageType in (
                        ImageType_RGB8_Planar, ImageType_YUV420SP_NV12,
                        ImageType_YUV420SP_NV21, ImageType_YUV422
                    ):
                        file_name = configMap.save_path("color","-_Color")
                        ret_save = camera.MV3D_RGBD_SaveImage(pointer(image_info), FileType_BMP, file_name)
                        print("Saved color image." if ret_save == MV3D_RGBD_OK else "Failed to save color image.")

                    # 保存灰度图
                    elif image_info.enImageType == ImageType_Mono8:
                        file_name = configMap.save_path("Mono","-_Mono")
                        ret_save = camera.MV3D_RGBD_SaveImage(pointer(image_info), FileType_BMP, file_name)
                        print("Saved mono image." if ret_save == MV3D_RGBD_OK else "Failed to save mono image.")

                    else:
                        print(f"Unknown image type: {image_info.enImageType}")

            else:
                print("Failed to fetch frame.")

            time.sleep(time_hop / 1000.0)  # 控制采集频率

    finally:
        # 停止取流
        camera.MV3D_RGBD_Stop()
        print("Continuous capture completed.")
    return saved_tiff_files


initialize_devices()

if __name__ == '__main__':
    pic("00DA6823936")