#include "camera.h"

#include <QTimer>

// Debug Options
#define GlobalDebug         0       // 全局是否允许打印Debug信息（打印会拖慢处理时间）
#define DebugDetection      0       // 注意开启这个编译选项会导致图片存储, 处理时间会很慢
#define DebugDetectionTime  0       // 是否打印处理时间
#define DebugLowerMacCOM    0       // 是否打印和下位机通讯的相关信息

camera::camera() {}

QTcpServer* server_to_lowermachine = nullptr;
QTcpSocket* lower_machine = nullptr;
bool volatile is_running = false;

MIL_ID MilApplication;
MIL_ID MilSystem;

static MIL_ID MilDigitizer0;
static MIL_ID MilImage0;
static MIL_ID MilImage_Color0;
static MIL_ID ModifiedBufferId0;
static MIL_ID MilGrabBufferList0[20] = {0};
static MIL_INT BufSizeX0 = 2048;
static MIL_INT BufSizeY0 = 512;
unsigned char* m_AvsBuffer0 = (unsigned char*)malloc(BufSizeX0 * BufSizeY0 * 3);
static int FuncCount0 = 1;

static MIL_ID MilDigitizer1;
static MIL_ID MilImage1;
static MIL_ID MilImage_Color1;
static MIL_ID ModifiedBufferId1;
static MIL_ID MilGrabBufferList1[20] = {0};
static MIL_INT BufSizeX1 = 2048;
static MIL_INT BufSizeY1 = 512;
unsigned char* m_AvsBuffer1 = (unsigned char*)malloc(BufSizeX1 * BufSizeY1 * 3);
static int FuncCount1 = 1;

int SaveImg_Flag;
ONNXRunner runner;


std::map<std::string, int> params;
int dual_cam_offset_y = 0;        // 双相机之间的上下偏移值
int widthBlocks = 20;             // 输出的喷阀通道数
int heightBlocks = 512;           // 输出的Mask高度
int sizeThreshold = 6;           // 转化为喷阀的每块要求像素个数
int expansionRaidus = 1;          // 获取mask后进行左右位置的扩展
int padLeft = 1;                  // 左侧在结果Mask上补0
int padRight = 1;                 // 右侧在结果Mask上补0
int rowRange = 1;                 // 结果维持行数（至少为1）
int ignoreSide = 0;               // 左右两侧的忽略的喷阀通道数量
int skipLeftCols = 0;             // 生成mask时跳过左侧的像素数量
int skipRightCols = 0;            // 生成mask时跳过右侧的像素数量

static std::vector<std::vector<uint8_t>> mask_0(0);
static std::vector<std::vector<uint8_t>> mask_1(0);

static std::vector<std::vector<uint8_t>> tail_0(0);
static std::vector<std::vector<uint8_t>> tail_1(0);


uint8_t temp_buf[512 * 64] = {0};

extern int file_delay;
extern int file_encoder;
extern int file_valve;

//下位机参数
extern int lowmac_dp;       //偏振延迟时间
extern int lowmac_sm;       //吹气量
extern int lowmac_ts;       //模板匹配阈值
extern int lowmac_sg;       //偏振绿色通道大小阈值
extern int lowmac_td;       //偏振红色通道差值

Timer CallBackTimer0;
Timer CallBackTimer1;

// ONNXRunner runner;
bool iniCamera()
{
    //分配application
    MappAlloc(M_DEFAULT, &MilApplication);

    //分配system
    MsysAlloc(M_DEFAULT, M_SYSTEM_RADIENTEVCL, M_DEV0, M_DEFAULT, &MilSystem);

    //分配相机 digitier
    QString config_dir = getConfigDirectory();
    MdigAlloc(MilSystem,M_DEV0,(config_dir + "/1.dcf").toStdWString(), M_DEFAULT,&MilDigitizer0);
    MdigAlloc(MilSystem,M_DEV1,(config_dir + "/2.dcf").toStdWString(), M_DEFAULT,&MilDigitizer1);

    //给MilImage分配空间
    MbufAllocColor(MilSystem,3,BufSizeX0,BufSizeY0,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage0);
    MbufAllocColor(MilSystem,3,BufSizeX1,BufSizeY1,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage1);
    MbufAllocColor(MilSystem,3,BufSizeX0,BufSizeY0,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage_Color0);
    MbufAllocColor(MilSystem,3,BufSizeX1,BufSizeY1,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage_Color1);


    //给每一个bufferlist分配空间
    for (int i = 0; i < 20; i++)
    {
        //              系统    3维度   宽      高        8位无符号       图像数据                    对象
        MbufAllocColor(MilSystem,3,BufSizeX0,BufSizeY0,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilGrabBufferList0[i]);
        if (MilGrabBufferList0[i])
        {
            MbufClear(MilGrabBufferList0[i], 0xFF);
        }
        else
        {
            break;
        }
    }

    for (int i = 0; i < 20; i++)
    {
        //              系统    3维度   宽      高        8位无符号       图像数据                    对象
        MbufAllocColor(MilSystem,3,BufSizeX1,BufSizeY1,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilGrabBufferList1[i]);
        if (MilGrabBufferList1[i])
        {
            MbufClear(MilGrabBufferList1[i], 0xFF);
        }
        else
        {
            break;
        }
    }
#if(GlobalDebug)
    qDebug()<<"ready";
#endif

    return 1;
}

// #if(GlobalDebug && DebugDetection)
// // #define SAVE_PATH_resize  MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//resize.png")
// #define SAVE_PATH_flip  MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//flip.png")
// #define SAVE_PATH_raw  MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//raw.png")
// #define SAVE_PATH_result  MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//result.png")
// #endif

MIL_INT ProcessingFunction0(MIL_INT HookType, MIL_ID HookId, void *HookDataPtr)
{
#if(GlobalDebug)
    qDebug()<<"CallBack1";
    Timer call_back_timer0;
    call_back_timer0.restart();
#endif

    int camera_id = 0;

#if(GlobalDebug && DebugDetectionTime)
    Timer timer_detection_time;
    timer_detection_time.restart();
#endif

    MdigGetHookInfo(HookId, M_MODIFIED_BUFFER + M_BUFFER_ID, &ModifiedBufferId0);
    {
        QMutexLocker locker(&gDispPicMutex0);
        gDispCurrentPicId0 = ModifiedBufferId0;
    }

#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack1: Send mil id for detection");
    timer_detection_time.restart();
#endif

    if (SaveImg_Flag)
    {
        // 拷贝存图图像
        MbufCopy(ModifiedBufferId0, MilImage0);
        cv::Mat img = ImageUtils::mil2Mat(MilImage0);
        // 将图像数据推入存储队列
        ImageData data;
        data.camera_id = 0;
        data.image = img.clone();   // 确保图像数据被复制
        g_storageQueue.enqueue(data);

#if(GlobalDebug && DebugDetectionTime)
        qDebug() << "CallBack1: push image to storage queue";
#endif
    }

    // 拷贝艳丽色检测图像
    MbufCopy(ModifiedBufferId0, MilImage_Color0);
    MIL_UNIQUE_BUF_ID MimResizedestination = MbufAllocColor(MilSystem, 3, 1024, 512, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_UNIQUE_ID);
    MbufClear(MimResizedestination, M_COLOR_BLACK);
    MimResize(MilImage_Color0, MimResizedestination, 0.5, 1 , M_DEFAULT);

#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack1: High sat detection resize");
    timer_detection_time.restart();
#endif

    // 图片镜像翻转
    MIL_UNIQUE_BUF_ID MimFlipDedtination = MbufClone(MimResizedestination, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_UNIQUE_ID);
    MbufClear(MimFlipDedtination, M_COLOR_BLACK);
    MimFlip(MimResizedestination, MimFlipDedtination, M_FLIP_HORIZONTAL, M_DEFAULT);
    cv::Mat img = ImageUtils::mil2Mat(MimFlipDedtination);

    // 将图像推入识别队列
    if(g_dl_enable[camera_id])
    {
        ImageData recognitionData;
        recognitionData.camera_id = camera_id;
        recognitionData.image = img;
        g_img_Queue[camera_id]->enqueue(recognitionData);
    }

#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack1: High sat detection mirror and push to DL");
    timer_detection_time.restart();
#endif

    // 艳丽检测mask
    Mat matrox_mat;
    std::vector<std::vector<uint8_t>> matrox_mask;
    if(g_traditional_enable[camera_id])
    {
        // 使用 std::async 将 high_sat_detect 封装为异步任务
        auto future = std::async(std::launch::async, [&]() {
            MIL_ID detection_result_id;
            high_sat_detect(MimFlipDedtination, detection_result_id, params);
            matrox_mat = ImageUtils::mil2Mat(detection_result_id);
            MbufFree(detection_result_id);
        });

        // 等待最多20毫秒
        if(future.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready)
        {
#if(GlobalDebug && DebugDetectionTime)
            timer_detection_time.printElapsedTime("CallBack1: High sat detection finish");
            timer_detection_time.restart();
#endif
            // 任务在指定时间内完成，可以继续使用 detection_result1 \
            // 将 Matrox 的检测结果转换为 二维vector
            matrox_mask = generateMaskFromMatImage(matrox_mat, widthBlocks, heightBlocks, sizeThreshold);
        }
        else
        {
            // 任务超时，处理超时情况
            qWarning() << "high_sat_detect 超时, 未在指定时间内完成。";
            // 创建一个大小为 widthBlocks x heightBlocks 的 matrox_mask
            matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
        }
    }
    else
    {
        matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
    }

#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack1: High sat detection to mask");
    timer_detection_time.restart();
#endif
    // 获取深度学习的检测结果并进行合并
    std::vector<std::vector<uint8_t>> merged_mask;
    std::vector<std::vector<uint8_t>> dl_mask;
    ImageData dl_data;

    if (g_dl_enable[camera_id])
    {
        // 使用 std::async 将深度学习检测封装为异步任务
        auto future_dl = std::async(std::launch::async, [&]() -> bool {
            // 尝试从队列中获取检测结果，假设 dequeue 支持超时
            // 如果 dequeue 不支持超时，请确保它不会无限期阻塞
            bool success = g_result_Queue[camera_id]->dequeue(dl_data);
            return success;
        });

        // 等待最多20毫秒
        if (future_dl.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready && future_dl.get())
        {
#if(GlobalDebug && DebugDetectionTime)
            timer_detection_time.printElapsedTime("CallBack1: DL detection finished within 20ms");
            timer_detection_time.restart();
#endif
            // 将深度学习的检测结果转换为 OpenCV Mat
            dl_mask = generateMaskFromMatImage(dl_data.image, widthBlocks, heightBlocks, sizeThreshold);
            merged_mask = ImageUtils::mergeMasks(dl_mask, matrox_mask);
        }
        else
        {
            // 任务超时或未成功获取结果，使用 Matrox 的检测结果
            qWarning() << "DL detection 超时或未成功获取结果 for camera" << camera_id;
            dl_mask = matrox_mask;
            merged_mask = matrox_mask;
#if(GlobalDebug && DebugDetectionTime)
            timer_detection_time.printElapsedTime("CallBack1: DL detection error or timeout to mask");
            timer_detection_time.restart();
#endif
        }
    }
    else
    {
        dl_mask = matrox_mask;
        merged_mask = matrox_mask;
    }

// Update the current Img MIl id
// 更新持久化存储
// {
//     QMutexLocker locker(&g_detection_result[camera_id].mutex);
//     g_detection_result[camera_id].dl_mask = dl_mask;
//     g_detection_result[camera_id].traditional_mask = matrox_mask;
// }
#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack1: Push result to g_detection_result");
    timer_detection_time.restart();
#endif
    mask_0 = merged_mask;
    detection_ready.release();

#if(GlobalDebug && DebugDetectionTime)
    call_back_timer0.printElapsedTime("CallBack1: Total time spent: ");
#endif
    return 0;
}


MIL_INT ProcessingFunction1(MIL_INT HookType, MIL_ID HookId, void *HookDataPtr)
{
#if(GlobalDebug)
    qDebug()<<"CallBack2";
    Timer call_back_timer1;
    call_back_timer1.restart();
#endif
    int camera_id = 1;
#if(GlobalDebug && DebugDetectionTime)
    Timer timer_detection_time;
    timer_detection_time.restart();
#endif
    MdigGetHookInfo(HookId, M_MODIFIED_BUFFER + M_BUFFER_ID, &ModifiedBufferId1);
    // Update the current Img MIl id for display
    {
        QMutexLocker locker(&gDispPicMutex1);
        gDispCurrentPicId1 = ModifiedBufferId1;
    }
#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack2: Send mil id for detection");
    timer_detection_time.restart();
#endif

    // 将图像数据推入存储队列
    if (SaveImg_Flag)
    {
        // 转回OpenCV图像
        MbufCopy(ModifiedBufferId1, MilImage1);
        cv::Mat img = ImageUtils::mil2Mat(MilImage1);
        ImageData data;
        data.camera_id = 1;
        data.image = img.clone(); // 确保图像数据被复制
        g_storageQueue.enqueue(data);
#if(GlobalDebug && DebugDetectionTime)
        qDebug() << "CallBack2: push image to storage queue";
#endif
    }

    //拷贝艳丽色检测图像
    MbufCopy(ModifiedBufferId1,MilImage_Color1);
    MIL_UNIQUE_BUF_ID MimResizedestination = MbufAllocColor(MilSystem, 3, 1024, 512, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_UNIQUE_ID);
    MbufClear(MimResizedestination, M_COLOR_BLACK);
    MimResize(MilImage_Color1, MimResizedestination, 0.5, 1 , M_DEFAULT);

#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack2: High sat detection resize");
    timer_detection_time.restart();
#endif

    if(g_dl_enable[camera_id])
    {
        // 将图像推入识别队列
        cv::Mat img = ImageUtils::mil2Mat(MimResizedestination);
        ImageData recognitionData;
        recognitionData.camera_id = camera_id;
        recognitionData.image = img;
        g_img_Queue[camera_id]->enqueue(recognitionData);
    }

#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack2: Without mirror and push to DL");
    timer_detection_time.restart();
#endif

    // 艳丽检测mask
    Mat matrox_mat;
    std::vector<std::vector<uint8_t>> matrox_mask;
    if(g_traditional_enable[camera_id])
    {
        // 使用 std::async 将 high_sat_detect 封装为异步任务
        auto future = std::async(std::launch::async, [&]() {
            MIL_ID detection_result_id;
            high_sat_detect(MimResizedestination, detection_result_id, params);
            matrox_mat = ImageUtils::mil2Mat(detection_result_id);
            MbufFree(detection_result_id);
        });

        // 等待最多20毫秒
        if(future.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready)
        {
#if(GlobalDebug && DebugDetectionTime)
            timer_detection_time.printElapsedTime("CallBack2: High sat detection finish");
            timer_detection_time.restart();
#endif
        // 任务在20ms内完成，可以继续使用 detection_result1 \
        // 将 Matrox 的检测结果转换为 二维vector
            matrox_mask = generateMaskFromMatImage(matrox_mat, widthBlocks, heightBlocks, sizeThreshold);        
            #if(GlobalDebug && DebugDetectionTime)
                timer_detection_time.printElapsedTime("CallBack2: High sat detection to mask");
                timer_detection_time.restart();
            #endif
        }
        else
        {
#if(GlobalDebug && DebugDetectionTime)
            timer_detection_time.printElapsedTime("CallBack2: High sat detection timeout");
            timer_detection_time.restart();
#endif
        // 任务超时，处理超时情况
            qWarning() << "high_sat_detect 超时,未在20ms内完成。";
            // 创建一个大小为 widthBlocks x heightBlocks 的 matrox_mask
            matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
        }
    }
    else
    {
         matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
    }

    // 获取深度学习的检测结果并进行合并
    std::vector<std::vector<uint8_t>> merged_mask;
    std::vector<std::vector<uint8_t>> dl_mask;
    ImageData dl_data;

    if (g_dl_enable[camera_id])
    {
        // 使用 std::async 将深度学习检测封装为异步任务
        auto future_dl = std::async(std::launch::async, [&]() -> bool {
            // 尝试从队列中获取检测结果，假设 dequeue 支持超时
            // 如果 dequeue 不支持超时，请确保它不会无限期阻塞
            bool success = g_result_Queue[camera_id]->dequeue(dl_data);
            return success;
        });

        // 等待最多20毫秒
        if (future_dl.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready && future_dl.get())
        {
#if(GlobalDebug && DebugDetectionTime)
            timer_detection_time.printElapsedTime("CallBack2: DL detection finished within 20ms");
            timer_detection_time.restart();
#endif
        // 将深度学习的检测结果转换为 OpenCV Mat
            dl_mask = generateMaskFromMatImage(dl_data.image, widthBlocks, heightBlocks, sizeThreshold);
            merged_mask = ImageUtils::mergeMasks(dl_mask, matrox_mask);
        }
        else
        {
            // 任务超时或未成功获取结果，使用 Matrox 的检测结果
            qWarning() << "DL detection 超时或未成功获取结果 for camera" << camera_id;
            dl_mask = matrox_mask;
            merged_mask = matrox_mask;
#if(GlobalDebug && DebugDetectionTime)
            timer_detection_time.printElapsedTime("CallBack2: DL detection error or timeout to mask");
            timer_detection_time.restart();
#endif
        }
    }
    else
    {
        dl_mask = matrox_mask;
        merged_mask = matrox_mask;
    }


// Update the current Img MIl id
// 更新持久化存储
#if(GlobalDebug && DebugDetectionTime)
    timer_detection_time.printElapsedTime("CallBack2: Push result to g_detection_result");
    timer_detection_time.restart();
#endif
    mask_1 = merged_mask;

#if(GlobalDebug && DebufgDetection)
    MbufSave(SAVE_PATH_flip,MimFlipDedtination);
    // MbufSave(SAVE_PATH_resize,MimResizedestination);
    MbufSave(SAVE_PATH_raw,MilImage_Color0);
    MbufSave(SAVE_PATH_result,detection_result0);
#endif
    // 等待另一个算法线程执行结束
    detection_ready.acquire();

    vector<vector<uint8_t>> mask_tail;

    merged_mask = ImageUtils::mergeMasks(mask_0, mask_1, dual_cam_offset_y);
    std::tie(merged_mask, mask_tail) = ImageUtils::extractROI(merged_mask, 0, 0, widthBlocks, heightBlocks);

#if(GlobalDebug && DebugDetection)
    int row_count = 0;
    std::cout << "<<<<<<<<<<<<<<<<<<<<<Mask_0<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
    for(auto row: mask_0)
    {   std::cout << row_count << " ";
        for (auto element: row)
        {
            std::cout << int(element) << " ";
        }
        std::cout << std::endl;
        row_count ++;
        if(row_count >3)
            break;
    }
    std::cout << ">>>>>>>>>>>>>>>>>>>>Mask_0>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" << std:: endl;
    row_count = 0;
    std::cout << "<<<<<<<<<<<<<<<<<<<<<Mask_1<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
    for(auto row: mask_1)
    {   std::cout << row_count << " ";
        for (auto element: row)
        {

            std::cout << int(element) << " ";
        }
        std::cout << std::endl;
        row_count ++;
        if(row_count >3)
            break;
    }
    std::cout << ">>>>>>>>>>>>>>>>>>>>Mask_1>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" << std:: endl;
    row_count = 0;
    std::cout << "<<<<<<<<<<<<<<<<<<<<<merged_mask<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
    for(auto row: merged_mask)
    {   std::cout << row_count << " ";
        for (auto element: row)
        {

            std::cout << int(element) << " ";
        }
        std::cout << std::endl;
        row_count ++;
        if(row_count >3)
            break;
    }
    std::cout << ">>>>>>>>>>>>>>>>>>>>merged_mask>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" << std:: endl;
#endif

    // 结果后处理Post Process
    // 将每个结果左右扩展，扩展半径默认为1
    auto mask_expaned = expandMaskHorizontally(merged_mask, expansionRaidus);
    // 将结果的左右补充上0，让物体大小符合要求
    PadColumns(mask_expaned, padLeft, padRight, 0);

    // ======= 新增：统计 mask_expaned 里的 1 的总个数，并累加到全局变量 =======
    {
        long long count_ones_this_time = 0;
        for(const auto & row : mask_expaned)
        {
            for(uint8_t val : row)
            {
                count_ones_this_time += (val == 1);
            }
        }
        g_valveActionCount += count_ones_this_time;
        // 如果你想调试打印，可以
        // qDebug() << "This callback1 new triggered bits =" << count_ones_this_time
        //          << ", total=" << g_valveActionCount;
    }
    // ======= 新增结束 =======
    //将mask扩展到合适发送的大小
    std::vector<std::vector<uint8_t>> mask_Total = expandArray(mask_expaned,64);

    // save masks
// #if(GlobalDebug && DebugDetection)
//     VectorToImg(mask_1,"C:/Users/Pc/Desktop/img/mask" + std::to_string(FuncCount1) + ".bmp");
//     VectorToImg(mask_1,"C:/Users/Pc/Desktop/img/mask_ignored" + std::to_string(FuncCount1) + ".bmp");
//     VectorToImg(mask_Total,"C:/Users/Pc/Desktop/img/mask_expended" + std::to_string(FuncCount1) + ".bmp");
// #endif
    // 发送到下位机
    bool result_Low = get_valve_data(mask_Total);
    if(!result_Low)
    {
        qWarning()<<"下位机发送失败";
    }


#if(GlobalDebug && DebugDetectionTime)
    call_back_timer1.printElapsedTime("CallBack2: Total time spent: ");
#endif
    return 0;
}


bool DestoryCamera()
{
    for (int i = 0; i < 20; i++)
    {
        MbufFree(MilGrabBufferList0[i]);
    }

    for (int i = 0; i < 20; i++)
    {
        MbufFree(MilGrabBufferList1[i]);
    }

    MbufFree(MilImage0);
    MbufFree(MilImage1);
    MbufFree(MilImage_Color0);
    MbufFree(MilImage_Color1);
    MdigFree(MilDigitizer0);
    MdigFree(MilDigitizer1);
    MsysFree(MilSystem);
    MappFree(MilApplication);

    return 1;
}

// Optimized LabProcess function
void lab_process_raw(const MIL_ID& inputImage, MIL_ID& outputImageLab, const std::map<std::string, int>& params,
                     const std::vector<std::string>& color_vector)
{
    MIL_ID MilLabImage = M_NULL, MilLChannel = M_NULL, MilAChannel = M_NULL, MilBChannel = M_NULL;
    MIL_ID lab_result=M_NULL;

    int denoising = params.at("lab_denoising");

    // Check number of bands
    MIL_INT NumBands = 0;
    MbufInquire(inputImage, M_SIZE_BAND, &NumBands);
    if (NumBands != 3)
    {
        printf("输入图像不是 3 通道图像，请提供彩色图像。\n");
        return;
    }

    // Inquire image properties once
    // MIL_ID MilSystem = MbufInquire(inputImage, M_OWNER_SYSTEM, M_NULL);
    MIL_INT SizeX = MbufInquire(inputImage, M_SIZE_X, M_NULL);
    MIL_INT SizeY = MbufInquire(inputImage, M_SIZE_Y, M_NULL);

    // Allocate buffer for Lab image
    MbufAllocColor(MilSystem, 3, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilLabImage);

    // Convert image from sRGB to Lab
    MimConvert(inputImage, MilLabImage, M_SRGB_TO_LAB);

    // Create child buffers for L, a, b channels
    MbufChildColor(MilLabImage, 0, &MilLChannel);
    MbufChildColor(MilLabImage, 1, &MilAChannel);
    MbufChildColor(MilLabImage, 2, &MilBChannel);

    // Allocate output image as 1-bit image
    MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &outputImageLab);
    MbufClear(outputImageLab, 0);  // Initialize to 0

    // Pre-allocate binary buffers as 1-bit images
    MIL_ID MilBinaryL = M_NULL, MilBinaryA = M_NULL, MilBinaryB = M_NULL, MilResultLab = M_NULL;
    MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilBinaryL);
    MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilBinaryA);
    MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilBinaryB);
    MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilResultLab);
    MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &lab_result);

    MbufClear(lab_result, M_COLOR_BLACK);
    // Iterate over colors
    // 遍历颜色
    for (const auto& color : color_vector) {
        // 构建参数键
        std::string L_min_key = color + "_L_min";
        std::string L_max_key = color + "_L_max";
        std::string a_min_key = color + "_a_min";
        std::string a_max_key = color + "_a_max";
        std::string b_min_key = color + "_b_min";
        std::string b_max_key = color + "_b_max";

        // 获取参数值
        int L_min = params.at(L_min_key);
        int L_max = params.at(L_max_key);
        int a_min = params.at(a_min_key);
        int a_max = params.at(a_max_key);
        int b_min = params.at(b_min_key);
        int b_max = params.at(b_max_key);
        std::vector<int> lab_min_ps = {L_min, a_min, b_min};
        std::vector<int> lab_max_ps = {L_max, a_max, b_max};

        std::vector<int> lab_min_cv = psLabToOpenCVLab(lab_min_ps);
        std::vector<int> lab_max_cv = psLabToOpenCVLab(lab_max_ps);

        L_min = lab_min_cv[0];
        L_max = lab_max_cv[0];
        a_min = lab_min_cv[1];
        a_max = lab_max_cv[1];
        b_min = lab_min_cv[2];
        b_max = lab_max_cv[2];

        // 对每个通道进行二值化
        MimBinarize(MilLChannel, MilBinaryL, M_IN_RANGE, L_min, L_max);
        MimBinarize(MilAChannel, MilBinaryA, M_IN_RANGE, a_min, a_max);
        MimBinarize(MilBChannel, MilBinaryB, M_IN_RANGE, b_min, b_max);

        // 合并阈值结果
        MimArith(MilBinaryL, MilBinaryA, MilResultLab, M_AND);
        MimArith(MilResultLab, MilBinaryB, MilResultLab, M_AND);


        // 与输出图像合并
        MimArith(lab_result, MilResultLab, lab_result, M_OR);

    }

    MimClose(lab_result, MilResultLab, denoising, M_BINARY);
    MimOpen(MilResultLab, outputImageLab, denoising, M_BINARY);

    // Free binary buffers
    MbufFree(MilBinaryL);
    MbufFree(MilBinaryA);
    MbufFree(MilBinaryB);
    MbufFree(MilResultLab);

    // Free resources
    MbufFree(MilLChannel);
    MbufFree(MilAChannel);
    MbufFree(MilBChannel);
    MbufFree(MilLabImage);
    MbufFree(lab_result);
}


void lab_process(const MIL_ID& inputImage, MIL_ID& outputImageLab, const std::map<std::string, int>& params) {
    const std::vector<std::string> colors = {"green", "blue", "orange", "black", "red", "purple"};
    lab_process_raw(inputImage, outputImageLab, params, colors);
}

vector<int> psLabToOpenCVLab(const vector<int>& lab_ps) {
    int l_ps = lab_ps[0];
    int a_ps = lab_ps[1];
    int b_ps = lab_ps[2];

    // Conversion formulas
    int l_cv = round((l_ps / 100.0) * 255.0);  // Scale L from 0-100 to 0-255
    int a_cv = round(((a_ps + 128.0) / 255.0) * 255.0);  // Shift and scale a
    int b_cv = round(((b_ps + 128.0) / 255.0) * 255.0);  // Shift and scale b

    return {l_cv, a_cv, b_cv};
}

vector<int> opencvLabToPsLab(const vector<int>& lab_cv) {
    int l_cv = lab_cv[0];
    int a_cv = lab_cv[1];
    int b_cv = lab_cv[2];

    // Conversion formulas
    int l_ps = round((l_cv / 255.0) * 100.0);  // Scale L from 0-255 to 0-100
    int a_ps = round((a_cv / 255.0) * 255.0 - 128.0);  // Scale and shift a
    int b_ps = round((b_cv / 255.0) * 255.0 - 128.0);  // Scale and shift b

    return {l_ps, a_ps, b_ps};
}

void hsv_process(const MIL_ID& inputImage, MIL_ID& outputImageHSV, const std::map<std::string, int>& params)
{
    MIL_ID MilHSVImage = M_NULL, MilHChannel = M_NULL, MilSChannel = M_NULL, MilVChannel = M_NULL;
    MIL_ID hsv_result = M_NULL;
    MIL_ID hsv_denoising = M_NULL;
    int saturationThreshold = params.at("saturation_threshold");
    int denoising = params.at("saturation_denoising");

    // 检查输入图像的通道数
    MIL_INT NumBands = 0;
    MbufInquire(inputImage, M_SIZE_BAND, &NumBands);
    if (NumBands != 3)
    {
        printf("输入图像不是 3 通道图像，请提供彩色图像。\n");
        return;
    }

    // 分配用于存储 HSV 图像的缓冲区
    MbufAllocColor(MbufInquire(inputImage, M_OWNER_SYSTEM, M_NULL), 3,
                   MbufInquire(inputImage, M_SIZE_X, M_NULL),
                   MbufInquire(inputImage, M_SIZE_Y, M_NULL),
                   8 + M_UNSIGNED,
                   M_IMAGE + M_PROC + M_DISP,
                   &MilHSVImage);

    // 将图像从 sRGB 转换到 HSV
    MimConvert(inputImage, MilHSVImage, M_RGB_TO_HSV);

    // 创建 HSV 通道的子缓冲区
    MbufChildColor(MilHSVImage, 0, &MilHChannel);
    MbufChildColor(MilHSVImage, 1, &MilSChannel);
    MbufChildColor(MilHSVImage, 2, &MilVChannel);

    // 分配输出图像缓冲区
    MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
                MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
                M_IMAGE + M_PROC + M_DISP, &hsv_result);
    MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
                MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
                M_IMAGE + M_PROC + M_DISP, &hsv_denoising);
    MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
                MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
                M_IMAGE + M_PROC + M_DISP, &outputImageHSV);

    // 对 S 通道进行阈值分割
    MimBinarize(MilSChannel, hsv_result, M_GREATER,
                saturationThreshold, M_NULL);

    MimClose(hsv_result, hsv_denoising, denoising, M_BINARY);
    MimOpen(hsv_denoising, outputImageHSV, denoising, M_BINARY);

    // 释放资源
    MbufFree(MilHChannel);
    MbufFree(MilSChannel);
    MbufFree(MilVChannel);
    MbufFree(MilHSVImage);
    MbufFree(hsv_result);
    MbufFree(hsv_denoising);
}


void high_sat_detect(const MIL_ID& inputImage, MIL_ID& outputImage, const std::map<std::string, int>& params) {
    MIL_ID output_hsv=M_NULL, output_lab=M_NULL;

    hsv_process(inputImage, output_hsv, params);
    lab_process(inputImage, output_lab, params);

    MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
                MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
                M_IMAGE + M_PROC, &outputImage);


    // 合并 Lab 和 HSV 的结果（取“或”运算）
    MimArith(output_hsv, output_lab, outputImage, M_OR);
    // MIL_ID MilBlobContext = M_NULL, MilBlobResult = M_NULL, MilFiltered = M_NULL;
    // MIL_INT SizeX = 0, SizeY = 0;
    // MIL_INT blob_count = 0;
    // MblobAlloc(MilSystem, M_DEFAULT, M_DEFAULT, &MilBlobContext);
    // MblobAllocResult(MilSystem, M_DEFAULT, M_DEFAULT, &MilBlobResult);
    // MblobCalculate(MilBlobContext, outputImage, M_NULL, MilBlobResult);
    // MblobSelect(MilBlobResult, M_EXCLUDE, M_AREA, M_LESS, MIN_AREA, M_NULL);
    // MblobGetResult(MilBlobResult, M_NUMBER, &blob_count);  // 修正：只传 M_NUMBER
    // // 获取 detection_result 的尺寸
    // MbufInquire(outputImage, M_SIZE_X, &SizeX);
    // MbufInquire(outputImage, M_SIZE_Y, &SizeY);
    // // 分配一个新的二值图像来存储筛选后的结果
    // MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilFiltered);
    // // 将 MilFiltered 初始化为 0（全黑）
    // MbufClear(MilFiltered, 0);
    // MIL_ID MilGraphicsContext;
    // MgraAlloc(MilSystem, &MilGraphicsContext);
    // MblobDraw(MilGraphicsContext, MilBlobResult, MilFiltered, M_DRAW_BLOBS, M_INCLUDED_BLOBS, M_DEFAULT);
    // // 将筛选后的结果复制回 detection_result
    // MbufCopy(MilFiltered, outputImage);
    MbufFree(output_lab);
    MbufFree(output_hsv);
}

void read_params_from_file(const std::string& filename, std::map<std::string, int>& params) {
    std::ifstream infile(filename);
    if (!infile) {
        std::cerr << "无法打开文件: " << filename << std::endl;
        return;
    }

    std::string line;
    while (std::getline(infile, line)) {
        // 去除行首和行尾的空白字符
        line.erase(0, line.find_first_not_of(" \t\r\n"));
        line.erase(line.find_last_not_of(" \t\r\n") + 1);

        // 跳过空行和注释行
        if (line.empty() || line[0] == '#')
            continue;

        // 查找等号的位置
        size_t pos = line.find('=');
        if (pos == std::string::npos)
            continue; // 如果没有等号，跳过该行

        // 分割键和值，并去除空白字符
        std::string key = line.substr(0, pos);
        std::string value_str = line.substr(pos + 1);
        key.erase(0, key.find_first_not_of(" \t"));
        key.erase(key.find_last_not_of(" \t") + 1);
        value_str.erase(0, value_str.find_first_not_of(" \t"));
        value_str.erase(value_str.find_last_not_of(" \t") + 1);

        // 将字符串转换为整数
        int value;
        std::istringstream iss(value_str);
        if (!(iss >> value)) {
            std::cerr << "键 " << key << " 的值无效: " << value_str << std::endl;
            continue;
        }

        // 将键值对添加到参数映射中
        params[key] = value;
    }
}

std::vector<std::vector<uint8_t>> generateMaskFromImage(const MIL_ID& inputImage, int widthBlocks, int heightBlocks, int thresholds= 10) {
    // 读取图像
    // cv::Mat image = cv::imread(imagePath, cv::IMREAD_GRAYSCALE);

    // // 检查图像是否成功读取
    // if (image.empty()) {
    //     std::cerr << "无法加载图像，请检查路径是否正确: " << imagePath << std::endl;
    //     exit(EXIT_FAILURE);
    // }
    cv::Mat image=ImageUtils::mil2Mat(inputImage);
    // 确保图像是二值化的
    cv::threshold(image, image, 128, 255, cv::THRESH_BINARY);

    // 获取图像的宽度和高度
    int imageWidth = image.cols;
    int imageHeight = image.rows;

    // 计算每个块的宽度和高度
    int blockWidth = imageWidth / widthBlocks;
    int blockHeight = imageHeight / heightBlocks;

    // 创建掩膜矩阵
    std::vector<std::vector<uint8_t>> mask_1(heightBlocks, std::vector<uint8_t>(widthBlocks, false));

    // 遍历每个块并统计白色像素点的数量
    for (int i = 0; i < heightBlocks; ++i) {
        for (int j = 0; j < widthBlocks; ++j) {
            // 计算块的起始和结束位置
            int x_start = j * blockWidth;
            int y_start = i * blockHeight;
            int x_end = (j == widthBlocks - 1) ? imageWidth : (j + 1) * blockWidth;
            int y_end = (i == heightBlocks - 1) ? imageHeight : (i + 1) * blockHeight;

            // 提取当前块
            cv::Mat block = image(cv::Rect(x_start, y_start, x_end - x_start, y_end - y_start));

            // 统计块中白色像素的数量
            int whitePixelCount = cv::countNonZero(block);

            // 如果白色像素数大于阈值，将该块标记为 true
            if (whitePixelCount > thresholds) {
                mask_1[i][j] = true;
            }
        }
    }

    return mask_1;

}

std::vector<std::vector<uint8_t>> generateMaskFromMatImage(const cv::Mat& image, int widthBlocks, int heightBlocks, int thresholds= 10) {

    // 确保图像是二值化的-*
    cv::threshold(image, image, 128, 255, cv::THRESH_BINARY);

    // 获取图像的宽度和高度
    int imageWidth = image.cols;
    int imageHeight = image.rows;

    // 计算每个块的宽度和高度
    int blockWidth = imageWidth / widthBlocks;
    int blockHeight = imageHeight / heightBlocks;

    // 创建掩膜矩阵
    std::vector<std::vector<uint8_t>> mask(heightBlocks, std::vector<uint8_t>(widthBlocks, false));

    // 遍历每个块并统计白色像素点的数量
    for (int i = 0; i < heightBlocks; ++i)
    {
        for (int j = 0; j < widthBlocks; ++j)
        {
            // 计算块的起始和结束位置
            int x_start = j * blockWidth;
            int y_start = i * blockHeight;
            int x_end = (j == widthBlocks - 1) ? imageWidth : (j + 1) * blockWidth;
            int y_end = (i == heightBlocks - 1) ? imageHeight : (i + 1) * blockHeight;

            // 提取当前块
            cv::Mat block = image(cv::Rect(x_start, y_start, x_end - x_start, y_end - y_start));

            // 统计块中白色像素的数量
            int whitePixelCount = cv::countNonZero(block);

            // 如果白色像素数大于阈值，将该块标记为 true
            if (whitePixelCount > thresholds)
            {
                mask[i][j] = true;
            }

        }
    }

    return mask;
}


bool iniColor()
{
    read_params_from_file((getConfigDirectory()+"/color_range_config.txt").toStdString(), params);
    return 1;
}


bool iniOnnx()
{

    std::string modelPath = (getConfigDirectory() + "/dimo_369_640.onnx").toStdString();

    runner.load(modelPath);

    // cv::Mat mask;
    // std::vector<Detection> result = runner.predict(image);
    // mask = runner.postProcess(result, image);
    // std::string savepath = "C:/Users/admin/Desktop/config/suspect_mask.png";
    // cv::imwrite(savepath, mask);
    return 1;
}

bool iniLowMac()
{
    /*--- server socket to faban ---*/
    server_to_lowermachine = new QTcpServer();
#if(GlobalDebug && DebugLowerMacCOM)
    qDebug()<<"1/4. Build Tcp Server";
#endif
    server_to_lowermachine->listen(QHostAddress::Any, 13452);
#if(GlobalDebug && DebugLowerMacCOM)
    qDebug()<<"2/4. Bind to Port 13452";
#endif
    bool is_timeout;
    server_to_lowermachine->waitForNewConnection(5000,&is_timeout);
#if(GlobalDebug && DebugLowerMacCOM)
    qDebug()<<"3/4. Try to Establish connect";
#endif

    if(is_timeout)
    {
        return 0;
    }
#if(GlobalDebug && DebugLowerMacCOM)
    qDebug()<<"4/4. Connection Established no timeout";
#endif

    lower_machine = server_to_lowermachine->nextPendingConnection();

#if(GlobalDebug && DebugLowerMacCOM)
    qDebug()<<"Lower Machine Connection Suecced";
#endif

    if (lower_machine == nullptr || !lower_machine->isWritable()) {
        cout << "Error: Lower machine is not available or writable." << endl;
        return 0 ;
    }



    // 计算 Y = 100000000 / X
    int divide_camera = (file_encoder != 0) ? 100000000 / file_encoder : 0;  // 防止除以零的情况
    int divide_valve = (file_valve != 0) ? 100000000 / file_valve : 0;  // 防止除以零的情况


    // 将参数转换为长度为8的字符串，前面补0
    QString delay_time = QString("%1").arg(file_delay, 8, 10, QChar('0'));
    QString divide_parameter = QString("%1").arg(divide_camera, 8, 10, QChar('0'));
    QString sv_parameter = QString("%1").arg(divide_valve, 8, 10, QChar('0'));

    int len_delay = delay_time.size();
    int len_divide = divide_parameter.size();
    int len_sv = sv_parameter.size();

    QByteArray delay_byte = delay_time.toLatin1();
    QByteArray divide_byte = divide_parameter.toLatin1();
    QByteArray sv_byte = sv_parameter.toLatin1();

    // 发送延迟时间
    uint8_t* delay_buf = new uint8_t[len_delay + 8];
    delay_buf[0] = 0xAA;
    delay_buf[1] = 0x00;
    delay_buf[2] = len_delay + 2;
    delay_buf[3] = 's';
    delay_buf[4] = 'd';
    memcpy(delay_buf + 5, delay_byte.data(), len_delay);
    delay_buf[len_delay + 5] = 0xFF;
    delay_buf[len_delay + 6] = 0xFF;
    delay_buf[len_delay + 7] = 0xBB;

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)delay_buf, len_delay + 8);
    } else {
        cout << "Error: Unable to write to lower machine for delay parameter." << endl;
    }
    delete[] delay_buf;

    // 发送相机参数
    uint8_t* divide_buf = new uint8_t[len_divide + 8];
    divide_buf[0] = 0xAA;
    divide_buf[1] = 0x00;
    divide_buf[2] = len_divide + 2;
    divide_buf[3] = 's';
    divide_buf[4] = 'c';
    memcpy(divide_buf + 5, divide_byte.data(), len_divide);
    divide_buf[len_divide + 5] = 0xFF;
    divide_buf[len_divide + 6] = 0xFF;
    divide_buf[len_divide + 7] = 0xBB;

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)divide_buf, len_divide + 8);
    } else {
        cout << "Error: Unable to write to lower machine for encoder parameter." << endl;
    }
    delete[] divide_buf;

    // 发送阀门参数
    uint8_t* valve_divide_buf = new uint8_t[len_sv + 8];
    valve_divide_buf[0] = 0xAA;
    valve_divide_buf[1] = 0x00;
    valve_divide_buf[2] = len_sv + 2;
    valve_divide_buf[3] = 's';
    valve_divide_buf[4] = 'v';
    memcpy(valve_divide_buf + 5, sv_byte.data(), len_sv);
    valve_divide_buf[len_sv + 5] = 0xFF;
    valve_divide_buf[len_sv + 6] = 0xFF;
    valve_divide_buf[len_sv + 7] = 0xBB;

    qDebug()<<"相机参数"<<divide_byte.data();
    qDebug()<<"阀门参数"<<sv_byte.data();

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)valve_divide_buf, len_sv + 8);
    } else {
        cout << "Error: Unable to write to lower machine for valve parameter." << endl;
    }
    delete[] valve_divide_buf;


    // //下位机参数
    // int lowmac_dp = 400;      //偏振延迟时间
    // int lowmac_sm = 5;        //吹气量
    // int lowmac_ts = 15;       //模板匹配阈值
    // int lowmac_sg = 80;       //偏振绿色通道大小阈值
    // int lowmac_td = 10;       //偏振红色通道差值
    //新增下位机协议
    // 将参数转换为长度为8的字符串，前面补0
    QString dp = QString("%1").arg(lowmac_dp, 8, 10, QChar('0'));
    QString sm = QString("%1").arg(lowmac_sm, 8, 10, QChar('0'));
    QString ts = QString("%1").arg(lowmac_ts, 8, 10, QChar('0'));
    QString sg = QString("%1").arg(lowmac_sg, 8, 10, QChar('0'));
    QString td = QString("%1").arg(lowmac_td, 8, 10, QChar('0'));

    int len_dp = dp.size();
    int len_sm = sm.size();
    int len_ts = ts.size();
    int len_sg = sg.size();
    int len_td = td.size();

    QByteArray dp_byte = dp.toLatin1();
    QByteArray sm_byte = sm.toLatin1();
    QByteArray ts_byte = ts.toLatin1();
    QByteArray sg_byte = sg.toLatin1();
    QByteArray td_byte = td.toLatin1();

    uint8_t* dp_buf = new uint8_t[len_dp + 8];
    dp_buf[0] = 0xAA;
    dp_buf[1] = 0x00;
    dp_buf[2] = len_dp + 2;
    dp_buf[3] = 'd';
    dp_buf[4] = 'p';
    memcpy(dp_buf + 5, dp_byte.data(), len_dp);
    dp_buf[len_dp + 5] = 0xFF;
    dp_buf[len_dp + 6] = 0xFF;
    dp_buf[len_dp + 7] = 0xBB;

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)dp_buf, len_dp + 8);
    } else {
        cout << "Error: Unable to write to lower machine for dp parameter." << endl;
    }
    delete[] dp_buf;

    uint8_t* sm_buf = new uint8_t[len_sm + 8];
    sm_buf[0] = 0xAA;
    sm_buf[1] = 0x00;
    sm_buf[2] = len_sm + 2;
    sm_buf[3] = 's';
    sm_buf[4] = 'm';
    memcpy(sm_buf + 5, sm_byte.data(), len_sm);
    sm_buf[len_sm + 5] = 0xFF;
    sm_buf[len_sm + 6] = 0xFF;
    sm_buf[len_sm + 7] = 0xBB;

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)sm_buf, len_sm + 8);
    } else {
        cout << "Error: Unable to write to lower machine for sm parameter." << endl;
    }
    delete[] sm_buf;

    uint8_t* ts_buf = new uint8_t[len_ts + 8];
    ts_buf[0] = 0xAA;
    ts_buf[1] = 0x00;
    ts_buf[2] = len_ts + 2;
    ts_buf[3] = 't';
    ts_buf[4] = 's';
    memcpy(ts_buf + 5, ts_byte.data(), len_ts);
    ts_buf[len_ts + 5] = 0xFF;
    ts_buf[len_ts + 6] = 0xFF;
    ts_buf[len_ts + 7] = 0xBB;

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)ts_buf, len_ts + 8);
    } else {
        cout << "Error: Unable to write to lower machine for ts parameter." << endl;
    }
    delete[] ts_buf;

    uint8_t* sg_buf = new uint8_t[len_sg + 8];
    sg_buf[0] = 0xAA;
    sg_buf[1] = 0x00;
    sg_buf[2] = len_sg + 2;
    sg_buf[3] = 's';
    sg_buf[4] = 'g';
    memcpy(sg_buf + 5, sg_byte.data(), len_sg);
    sg_buf[len_sg + 5] = 0xFF;
    sg_buf[len_sg + 6] = 0xFF;
    sg_buf[len_sg + 7] = 0xBB;

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)sg_buf, len_sg + 8);
    } else {
        cout << "Error: Unable to write to lower machine for sg parameter." << endl;
    }
    delete[] sg_buf;

    uint8_t* td_buf = new uint8_t[len_td + 8];
    td_buf[0] = 0xAA;
    td_buf[1] = 0x00;
    td_buf[2] = len_td + 2;
    td_buf[3] = 't';
    td_buf[4] = 'd';
    memcpy(td_buf + 5, td_byte.data(), len_td);
    td_buf[len_td + 5] = 0xFF;
    td_buf[len_td + 6] = 0xFF;
    td_buf[len_td + 7] = 0xBB;

    if (lower_machine->isWritable()) {
        lower_machine->write((const char*)td_buf, len_td + 8);
    } else {
        cout << "Error: Unable to write to lower machine for td parameter." << endl;
    }
    delete[] td_buf;

    return 1;
}

bool DestoryLowMac()
{
    MdigProcess(MilDigitizer0, MilGrabBufferList0, 20, M_STOP, M_DEFAULT,ProcessingFunction0, M_NULL);
    MdigProcess(MilDigitizer1, MilGrabBufferList1, 20, M_STOP, M_DEFAULT,ProcessingFunction1, M_NULL);

    // 构建停止命令
    uint8_t stop_command[9] = {0};
    stop_command[0] = 0xAA;  // 起始标志
    stop_command[1] = 0x00;  // 长度高位
    stop_command[2] = 0x03;  // 长度低位
    stop_command[3] = 's';   // 命令类型
    stop_command[4] = 'p';   // 停止命令
    stop_command[5] = 0xFF;  // 校验位1
    stop_command[6] = 0xFF;  // 校验位2
    stop_command[7] = 0xFF;  // 校验位3
    stop_command[8] = 0xBB;  // 结束标志

    // 发送停止命令给下位机
    if(lower_machine != nullptr && lower_machine->isWritable())
    {
        lower_machine->write(reinterpret_cast<const char*>(stop_command), 9);
        lower_machine->flush();
    }
    // 设置运行状态为 false
    is_running = false;
}


bool get_valve_data(std::vector<std::vector<uint8_t>> mask)
{
    if (mask[0].size() % 8 != 0) {
        std::cerr << "Error: mask 的第 0 行的列数应该为 8 的倍数。" << std::endl;
        return false;
    }
    uint8_t* mask_buf = new uint8_t[4096 + 8];  // 创建缓冲区，大小为3072 + 8
    mask_buf[0] = 0xAA;                         // 起始标志
    mask_buf[1] = 0x10;                         // 高位数据长度 (352 字节 -> 0x0160)
    mask_buf[2] = 0x02;                         // 低位数据长度
    mask_buf[3] = 'd';                          // 命令类型 (发送类型 'd')
    mask_buf[4] = 'a';                          // 数据类型 (阀门相关 'a')

    // 将二维容器中的二值数据转换为字节并存储到 mask_buf 中
    int idx = 5;  // 从 mask_buf[5] 开始存储数据
    for (int i = 0; i < 512; i++)  // 遍历512行
    {
        for (int j = 0; j < int(64 / 8); j++)  // 遍历64列
        {
            uint8_t byte = 0;
            for (int bit_idx = 0; bit_idx < 8; bit_idx++)
            {
                byte |= (mask[i][ j * 8 + bit_idx] & 0x01) << bit_idx;  // 每个字节内，低字节优先
            }
            mask_buf[idx++] = byte;
        }
    }

    mask_buf[4101] = 0xFF;  // 校验低位
    mask_buf[4102] = 0xFF;  // 校验高位
    mask_buf[4103] = 0xBB;  // 结束标志
#if(GlobalDebug && DebugLowerMacCOM)
    for (int i = 0; i <= 4103; i++)
    {
        // 将 mask_buf[i] 转换为 int 再输出，避免其被当作字符解释
        std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(mask_buf[i]) << " ";
    }
    std::cout << std::endl;
#endif

    // qDebug()<<&mask_buf;

    // qDebug() << "Sending data to lower machine in binary:";
    // for (int i = 0; i < 8200; ++i)
    // {
    //     QString binaryString = QString::number(mask_buf[i], 2).rightJustified(8, '0');
    //     qDebug() << QString("Byte %1: %2").arg(i).arg(binaryString);
    // }

    // 检查与设备的连接状态
    if (lower_machine != nullptr && lower_machine->state() == QAbstractSocket::ConnectedState)
    {
        lower_machine->write((const char*)mask_buf, 4104);  // 总共 3080 字节
        lower_machine->flush();
    }
    else
    {
        std::cout << "*** lower machine connect failed! *** " << std::endl;
        // ui->lab_lowermachine_isconnect->setStyleSheet("QLabel{background-color: rgb(237, 212, 0);}");  // 显示连接失败
        // ui->lab_lowermachine_isconnect->repaint();  // 强制刷新UI
    }

    delete[] mask_buf;  // 释放内存

    return true;
}


void Start_camera()
{
    MdigProcess(MilDigitizer0, MilGrabBufferList0, 20, M_START, M_DEFAULT,ProcessingFunction0, M_NULL);
    MdigProcess(MilDigitizer1, MilGrabBufferList1, 20, M_START, M_DEFAULT,ProcessingFunction1, M_NULL);

    // 发送开始命令
    uint8_t start_command[9] = {0};
    start_command[0] = 0xAA;
    start_command[1] = 0x00;
    start_command[2] = 0x03;
    start_command[3] = 's';
    start_command[4] = 't';
    start_command[5] = 0xFF;
    start_command[6] = 0xFF;
    start_command[7] = 0xFF;
    start_command[8] = 0xBB;

    if (lower_machine != nullptr && lower_machine->isWritable())
    {
        lower_machine->write((const char*)start_command, 9);
        qDebug()<<"发送相机参数成功";
    }
    else
    {
        cout << "Error: Unable to write to lower machine for start command." << endl;
    }
}



std::vector<std::vector<uint8_t> > expandArray(const std::vector<std::vector<uint8_t> > &array, int newCols)
{
    int rows = array.size();
    // 创建一个新的二维数组，初始化为0，列数为 newCols
    std::vector<std::vector<uint8_t>> array_total(rows, std::vector<uint8_t>(newCols, 0));

    // 将原数组的值复制到新数组的前22列
    for (int i = 0; i < rows; ++i)
    {
        for (int j = 0; j < array[i].size(); ++j)
        {
            array_total[i][j] = array[i][j];
        }
    }

    return array_total;
}


vector<vector<uint8_t>> generateMask(
    const MIL_ID& inputImg,
    int outputWidth,
    int outputHeight,
    int sizeThreshold,
    int skipLeftCols,
    int skipRightCols
    ) {
    cv::Mat image = ImageUtils::mil2Mat(inputImg);

    // Ensure the image is binary
    cv::threshold(image, image, 128, 255, cv::THRESH_BINARY);

    int imageWidth = image.cols;
    int imageHeight = image.rows;

    // Adjust image width by excluding skipLeftCols and skipRightCols
    int effectiveWidth = imageWidth - skipLeftCols - skipRightCols;
    if (effectiveWidth <= 0) {
        throw std::invalid_argument("Invalid column skip values. Effective width is less than or equal to zero.");
    }

    int blockWidth = effectiveWidth / outputWidth;
    int blockHeight = imageHeight / outputHeight;

    vector<vector<uint8_t>> mask(outputHeight, vector<uint8_t>(outputWidth, 0));

    for (int i = 0; i < outputHeight; ++i) {
        for (int j = 0; j < outputWidth; ++j) {
            int x_start = skipLeftCols + j * blockWidth;
            int y_start = i * blockHeight;
            int x_end = (j == outputWidth - 1) ? (imageWidth - skipRightCols) : (skipLeftCols + (j + 1) * blockWidth);
            int y_end = (i == outputHeight - 1) ? imageHeight : (i + 1) * blockHeight;

            cv::Mat block = image(cv::Rect(x_start, y_start, x_end - x_start, y_end - y_start));

            int whitePixelCount = cv::countNonZero(block);
            if (whitePixelCount > sizeThreshold) {
                mask[i][j] = 1;
            }
        }
    }

    return mask;
}


pair<vector<vector<uint8_t>>, vector<vector<uint8_t>>> applyRowRangeDelay(
    const vector<vector<uint8_t>>& mask,
    const vector<vector<uint8_t>>& tail,
    int rowRange
    )
{
    if(rowRange <= 0)
    {
        rowRange = 1;
        qDebug() << "Maintain Row Range Error, forced to 1!!!!!!!!";
    }
    int outputHeight = (int)mask.size();
    int outputWidth = (int)mask[0].size();

    vector<vector<uint8_t>> mask_after_row_range(outputHeight, vector<uint8_t>(outputWidth, 0));
    vector<vector<uint8_t>> newTail(rowRange, vector<uint8_t>(outputWidth, 0));

    // 先将旧的 tail 映射到 mask_after_row_range 的顶部几行
    for (int i = 0; i < (int)tail.size(); ++i) {
        for (int j = 0; j < outputWidth; ++j) {
            if (i < outputHeight) {
                mask_after_row_range[i][j] = max(mask_after_row_range[i][j], tail[i][j]);
            }
        }
    }

    // 对当前 mask 应用 rowRange 的拖影效果
    for (int j = 0; j < outputWidth; ++j) {
        for (int i = 0; i < outputHeight; ++i) {
            if (mask[i][j] == 1) {
                // 从当前行 i 开始，向下扩展 rowRange 行
                int end_line = i + rowRange - 1;

                // 先处理仍在 mask 范围内的部分
                int inside_mask_end = min(end_line, outputHeight - 1);
                for (int line = i; line <= inside_mask_end; ++line) {
                    mask_after_row_range[line][j] = 1;
                }

                // 超出 mask 范围的行进入 tail
                if (end_line >= outputHeight) {
                    // 从 outputHeight 行开始的部分属于 tail
                    for (int line = outputHeight; line <= end_line; ++line) {
                        int tail_line_idx = line - outputHeight;
                        if (tail_line_idx >= 0 && tail_line_idx < (int)newTail.size()) {
                            newTail[tail_line_idx][j] = 1;
                        }
                    }
                }
            }
        }
    }

    return {mask_after_row_range, newTail};
}


// Updated wrapper function
pair<vector<vector<uint8_t>>, vector<vector<uint8_t>>> generateMaskWithTail(
    const MIL_ID& inputImg,
    const vector<vector<uint8_t>>& tail,
    int outputWidth,
    int outputHeight,
    int sizeThreshold = 10,
    int rowRange=1,
    int skipLeftCols=10,
    int skipRightCols=10
    ) {
    // Generate the mask from the image
    vector<vector<uint8_t>> mask = generateMask(inputImg, outputWidth, outputHeight, sizeThreshold, skipLeftCols,skipRightCols);

    // Apply rowRange delay
    return applyRowRangeDelay(mask, tail, rowRange);
}



void PadColumns(std::vector<std::vector<uint8_t>>& data, int pad_left, int pad_right, uint8_t fill_value = 0)
{
    // 如果不需要填充，直接返回
    if (pad_left <= 0 && pad_right <= 0) {
        return;
    }

    for (auto& row : data) {
        // 在左侧插入pad_left个fill_value
        row.insert(row.begin(), pad_left, fill_value);
        // 在右侧插入pad_right个fill_value
        row.insert(row.end(), pad_right, fill_value);
    }
}


std::vector<std::vector<uint8_t>> expandMaskHorizontally(
    const std::vector<std::vector<uint8_t>>& mask,
    int expansionRadius)
{
    std::vector<std::vector<uint8_t>> expanded_mask = mask;
    int rows = (int)mask.size();
    if (rows == 0) return expanded_mask;
    int cols = (int)mask[0].size();

    for (int y = 0; y < rows; y++)
    {
        for (int x = 0; x < cols; x++)
        {
            if (mask[y][x] == 1)
            {
                for (int i = 1; i <= expansionRadius; i++)
                {
                    if (x - i >= 0)
                        expanded_mask[y][x - i] = 1;
                    if (x + i < cols)
                        expanded_mask[y][x + i] = 1;
                }
            }
        }
    }

    return expanded_mask;
}