// HessianTable.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <vector>
#include <cmath>
#include <corecrt_math_defines.h>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstring>
//#include <stdio.h>
//#include <stdint.h>

using namespace std;

#if 0
// 生成1D无偏高斯二阶导数掩模
vector<double> generate_1d_mask(double sigma, int radius) {
    int size = 2 * radius + 1;
    vector<double> mask(size, 0.0);
    double sigma_sq = sigma * sigma;
    double normalization = 1.0 / (sqrt(2 * M_PI) * sigma_sq * sigma);

    for (int i = -radius; i <= radius; ++i) {
        double x_low = i - 0.5;
        double x_high = i + 0.5;

        // 计算高斯二阶导数在区间[x_low, x_high]的积分
        double term1 = x_low * exp(-x_low * x_low / (2 * sigma_sq));
        double term2 = x_high * exp(-x_high * x_high / (2 * sigma_sq));
        double integral = (term1 - term2) / sigma_sq;

        // 积分第二部分：误差函数处理常数项
        double erf_term = erf(x_high / (sigma * sqrt(2))) - erf(x_low / (sigma * sqrt(2)));
        integral += erf_term * sigma * sqrt(M_PI / 2);

        integral *= normalization;
        mask[i + radius] = integral;
    }

    return mask;
}

// 1D卷积函数（处理边界为0扩展）
vector<double> convolve_1d(const vector<double>& signal, const vector<double>& mask) {
    int mask_size = mask.size();
    int radius = (mask_size - 1) / 2;
    vector<double> result(signal.size(), 0.0);

    for (int i = 0; i < signal.size(); ++i) {
        double sum = 0.0;
        for (int j = -radius; j <= radius; ++j) {
            int pos = i + j;
            if (pos >= 0 && pos < signal.size()) {
                sum += signal[pos] * mask[j + radius];
            }
        }
        result[i] = sum;
    }
    return result;
}

int main() {
    // 示例参数
    double sigma = 2.0;
    int radius = static_cast<int>(ceil(3 * sigma));

    // 生成掩模
    vector<double> mask = generate_1d_mask(sigma, radius);

    // 输出掩模系数
    cout << "Convolution Mask Coefficients:\n";
    for (double coeff : mask) {
        cout << coeff << " ";
    }
    cout << endl;

    // 示例信号（可根据需要修改）
    vector<double> signal = { 0, 0, 0, 1, 1, 1, 0, 0, 0 };

    // 应用卷积
    vector<double> response = convolve_1d(signal, mask);

    // 输出响应
    cout << "Convolution Response:\n";
    for (double val : response) {
        cout << val << " ";
    }
    cout << endl;

    return 0;
}

#include <vector>
#include <cmath>
#include <iostream>

using namespace std;

// 生成1D无偏高斯一阶导数掩模
vector<double> generate_1d_first_derivative_mask(double sigma, int radius) {
    int size = 2 * radius + 1;
    vector<double> mask(size, 0.0);
    double sigma_sq = sigma * sigma;
    double normalization = 1.0 / (sigma * sqrt(2 * M_PI)); // 高斯函数归一化因子

    for (int i = -radius; i <= radius; ++i) {
        double x_low = i - 0.5;
        double x_high = i + 0.5;

        // 计算高斯函数在离散区间端点的值
        double g_low = exp(-x_low * x_low / (2 * sigma_sq)) * normalization;
        double g_high = exp(-x_high * x_high / (2 * sigma_sq)) * normalization;

        // 积分结果 = G(x_low) - G(x_high)
        double integral = g_low - g_high;
        mask[i + radius] = integral;
    }

    return mask;
}

// 卷积函数（与之前的二阶实现相同）
vector<double> convolve_1d(const vector<double>& signal, const vector<double>& mask) {
    int mask_size = mask.size();
    int radius = (mask_size - 1) / 2;
    vector<double> result(signal.size(), 0.0);

    for (int i = 0; i < signal.size(); ++i) {
        double sum = 0.0;
        for (int j = -radius; j <= radius; ++j) {
            int pos = i + j;
            if (pos >= 0 && pos < signal.size()) {
                sum += signal[pos] * mask[j + radius];
            }
        }
        result[i] = sum;
    }
    return result;
}

int main() {
    // 示例参数
    double sigma = 2.0;
    int radius = static_cast<int>(ceil(3 * sigma));

    // 生成一阶导数掩模
    vector<double> mask_1st = generate_1d_first_derivative_mask(sigma, radius);

    // 输出掩模系数
    cout << "1st Derivative Mask Coefficients:\n";
    for (double coeff : mask_1st) {
        cout << coeff << " ";
    }
    cout << endl;

    // 示例信号（阶跃边缘）
    vector<double> signal = { 0, 0, 0, 0, 1, 1, 1, 1, 1 };

    // 计算一阶导数响应
    vector<double> response = convolve_1d(signal, mask_1st);

    // 输出响应
    cout << "1st Derivative Response:\n";
    for (double val : response) {
        cout << val << " ";
    }
    cout << endl;

    return 0;
}
#endif
#define LUMA_RGN_DATA_WIN_SIZE  16
#define LUMA_GEN_DATA_SCALE     100
typedef struct
{
    unsigned short WinRdx;//窗口在图像内的起始坐标
    unsigned short y; //y坐标
    unsigned short Rid;  //region的ID
    unsigned char Flag;  //bit0是overlap标志。bit1是反光信号标志
    unsigned char PeakRltvRdx; //peak在窗口内的的相对坐标，低4位是起始坐标，高4位是结束坐标
    unsigned char data[LUMA_RGN_DATA_WIN_SIZE];
    double offset;
}Luma_rgnData;

typedef struct
{
    double true_offset;
    double compute_offset;
    double nCom_offset;
}Luma_offsetTestCompare;

//生成测试数据：
//sigma:(1.0,2.0,3.0,4.0,5.0),
//Peak:（20，40，60，100，200，250），
//Offset: (-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4)
//每次调用生成Offset遍历数据(-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4)
std::vector<Luma_rgnData> _genTestData(double sigma, double peakValue)
{
    //int size = 10;
    //double offsetTable[10] = {-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4};
    int size = 20;
    double offsetTable[20] = { -0.5,-0.45,-0.4,-0.35, -0.3,-0.25, -0.2,-0.15, -0.1, -0.05, 
                            0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45 };
    std::vector< Luma_rgnData> result;

    int buffSize = (LUMA_RGN_DATA_WIN_SIZE + 3) * LUMA_GEN_DATA_SCALE;
    std::vector<double> dataBuffer(buffSize, 0);
    
    int radius = (LUMA_RGN_DATA_WIN_SIZE + 3)/2;
    int id = 0;
    double sigma2 = sigma * sigma;
    for (int i = -radius; i <= radius; i++)
    {
        for (int j = 0; j < LUMA_GEN_DATA_SCALE; j++)
        {
            double x = (double)i + (double)j / LUMA_GEN_DATA_SCALE;
            dataBuffer[id] = exp(-x * x / (2 * sigma2));
            id++;
        }
    }
    //生成结果数据
    int dataWin = LUMA_RGN_DATA_WIN_SIZE / 2;
    int y = 0;
    for (int n = 0; n < size; n++) //取不同的offset进行计算
    {
        double offset = offsetTable[n];
        int offsetPos = (int)(-offset * LUMA_GEN_DATA_SCALE) - LUMA_GEN_DATA_SCALE/2; //位于中心
        double rsltData[LUMA_RGN_DATA_WIN_SIZE];
        for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)//第个测试样本有16个数据，高斯分布，偏离位置为offset
        {
            double sum = 0;
            int pos = offsetPos + (i-dataWin) * LUMA_GEN_DATA_SCALE + radius * LUMA_GEN_DATA_SCALE;
            for (int j = 0; j < LUMA_GEN_DATA_SCALE; j++)  //过采样，间隔为0.01
                sum += dataBuffer[j + pos];
            rsltData[i] = sum;
        }
        double maxData = 0;
        for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
        {
            if (maxData < rsltData[i])
                maxData = rsltData[i];
        }
        Luma_rgnData a_sample;
        memset(&a_sample, 0, sizeof(Luma_rgnData));
        a_sample.y = y;
        y++;
        a_sample.Rid = 1;
        a_sample.PeakRltvRdx = 0xC3;
        a_sample.offset = offset;
        for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
        {
            double d = (rsltData[i] / maxData) * peakValue;
            if (d > 254.0)
                a_sample.data[i] = 254.0;
            else
                a_sample.data[i] = (unsigned char)d;
        }
        result.push_back(a_sample);
    }
    return result;
}

//直接采样法
// 高斯一阶导数掩模生成
vector<double> generate_1st_mask_directSample(double sigma, int radius) {
    int size = 2 * radius + 1;
    vector<double> mask(size, 0.0);
    double sigma2 = sigma * sigma;
    double norm = 1.0 / (sigma * sigma2 * sqrt(2 * M_PI));

    for (int i = -radius; i <= radius; ++i) {
        double x = i;
        mask[i + radius] = -x * exp(-x * x / (2 * sigma2));
        mask[i + radius] *= norm;
    }
    return mask;
}
// 高斯二阶导数掩模生成
vector<double> generate_2nd_mask_directSample(double sigma, int radius) {
    int size = 2 * radius + 1;
    vector<double> mask(size, 0.0);
    double sigma2 = sigma * sigma;
    double sigma4 = sigma2 * sigma2;
    double norm = 1.0 / (sqrt(2 * M_PI) * sigma * sigma4);

    for (int i = -radius; i <= radius; ++i) {
        double x = i;
        mask[i + radius] = (x  * x - sigma2) * exp(-x * x / (2 * sigma2));
        mask[i + radius] *= norm;
    }
    return mask;
}

//积分法
// 高斯一阶导数掩模生成
vector<double> generate_1st_mask_Integral(double sigma, int radius) {
    int size = 2 * radius + 1;
    vector<double> mask(size, 0.0);
    double norm = 1.0 / (sigma * sqrt(2 * M_PI));
    for (int i = -radius; i <= radius; ++i) {
        double x_low = i - 0.5;
        double x_high = i + 0.5;
        mask[i + radius] = exp(-x_high * x_high / (2 * sigma * sigma)) - 
            exp(-x_low * x_low / (2 * sigma * sigma));
        mask[i + radius] *= norm;
    }
    return mask;
}

// 高斯二阶导数掩模生成
vector<double> generate_2nd_mask_Integral(double sigma, int radius) {
    int size = 2 * radius + 1;
    vector<double> mask(size, 0.0);
    double sigma2 = sigma * sigma;
    double norm = 1.0 / (sqrt(2 * M_PI) * sigma * sigma2);

    for (int i = -radius; i <= radius; ++i) {
        double x_low = i - 0.5;
        double x_high = i + 0.5;
        double term1 = x_low * exp(-x_low * x_low / (2 * sigma2));
        double term2 = x_high * exp(-x_high * x_high / (2 * sigma2));
        mask[i + radius] = term1 - term2;
        mask[i + radius] *= norm;
    }
    return mask;
}

// 卷积函数
vector<double> convolve(const vector<double>& signal, const vector<double>& mask) {
    int radius = (mask.size() - 1) / 2;
    vector<double> result(signal.size(), 0.0);

    for (int i = 0; i < signal.size(); ++i) {
        double sum = 0.0;
        for (int j = -radius; j <= radius; ++j) {
            int pos = i + j;
            if (pos >= 0 && pos < signal.size()) {
                sum += signal[pos] * mask[j + radius];
            }
        }
        result[i] = sum;
    }
    return result;
}

void _genSubpixMask(
    double sigma,
    int radius,
    std::vector<double>& outMask_1st,
    std::vector<double>& outMask_2nd)
{
    // 生成掩模
    auto mask_1st = generate_1st_mask_directSample(sigma, radius);
    auto mask_2nd = generate_2nd_mask_directSample(sigma, radius);
    auto mask_1st_integ = generate_1st_mask_Integral(sigma, radius);
    auto mask_2nd_integ = generate_2nd_mask_Integral(sigma, radius);
    //生成宽度为LUMA_RGN_DATA_WIN_SIZE的mask
    std::vector<double> objMask_1st(LUMA_RGN_DATA_WIN_SIZE, 0.0);
    int size_1st = mask_1st_integ.size();
    for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
    {
        int pos = size_1st / 2 - LUMA_RGN_DATA_WIN_SIZE / 2 + i;
        if ((pos < 0) || (pos >= size_1st))
            objMask_1st[i] = 0;
        else
            objMask_1st[i] = mask_1st_integ[pos];
    }
    objMask_1st[0] = 0;
    for (int i = 0, i_max = objMask_1st.size(); i < i_max; i++)
        outMask_1st.push_back(objMask_1st[i]);
    std::vector<double> objMask_2nd(LUMA_RGN_DATA_WIN_SIZE, 0.0);
    int size_2nd = mask_2nd_integ.size();
    for (int i = 0; i < LUMA_RGN_DATA_WIN_SIZE; i++)
    {
        int pos = size_2nd / 2 - LUMA_RGN_DATA_WIN_SIZE / 2 + i;
        if ((pos < 0) || (pos >= size_2nd))
            objMask_2nd[i] = 0;
        else
            objMask_2nd[i] = mask_2nd_integ[pos];
    }
    objMask_2nd[0] = 0;
    for (int i = 0, i_max = objMask_2nd.size(); i < i_max; i++)
        outMask_2nd.push_back(objMask_2nd[i]);
    return;
}

// 亚像素定位核心算法
vector<Luma_offsetTestCompare> subpixel_localization(
    const std::vector<Luma_rgnData> signal,
    std::vector<double>& objMask_1st,
    std::vector<double>& objMask_2nd,
    std::vector<int>& nObjMask_1st,
    std::vector<int>& nObjMask_2nd)
{
    // 计算导数响应
    std::vector<Luma_offsetTestCompare> subpixel;
    for (int i = 0, i_max = signal.size(); i < i_max; i++)
    {
        double D = 0;
        double D2 = 0;
        int nD = 0;
        int nD2 = 0;
        for (int j = 0; j < LUMA_RGN_DATA_WIN_SIZE; j++)
        {
            D += (double)signal[i].data[j] * objMask_1st[j];
            D2 += (double)signal[i].data[j] * objMask_2nd[j];
            nD += (int)signal[i].data[j] * nObjMask_1st[j];
            nD2 += (int)signal[i].data[j] * nObjMask_2nd[j];
        }
        Luma_offsetTestCompare a_sub;
        a_sub.true_offset = signal[i].offset;
        a_sub.compute_offset = D / D2;
        a_sub.nCom_offset = (double)((float)nD / (float)nD2);
        subpixel.push_back(a_sub);
    }
    return subpixel;

#if 0
    vector<double> D = convolve(signal, mask_1st);
    vector<double> D2 = convolve(signal, mask_2nd);

    // 寻找候选点 (过零点)
    vector<double> positions;
    for (int i = 1; i < D.size() - 1; ++i) {
        // 检测过零点
        if (D[i] * D[i + 1] < 0 || D[i] * D[i - 1] < 0) {
            // 亚像素插值
            double x0 = D[i - 1];
            double x1 = D[i];
            double x2 = D[i + 1];

            // 抛物线插值公式
            double offset = 0.5 * (x0 - x2) / (x0 - 2 * x1 + x2);
            double subpixel_pos = i + offset;

            // 计算二阶导数验证
            double d2_val = D2[i] + offset * (D2[i + 1] - D2[i]);

            // 有效性检查 (排除弱响应)
            if (fabs(d2_val) > 1e-3) {
                positions.push_back(subpixel_pos);
            }
        }
    }
    return positions;
#endif
}

void _outputTestSample(char* fileName, std::vector<Luma_rgnData>& testSamples)
{
    std::ofstream sw(fileName);
    char str[250];
    sprintf_s(str, "0x10");
    sw << str << std::endl;
    for (int i = 0, i_max = testSamples.size(); i < i_max; i++)
    {
        char data[250];
        sprintf_s(data, "%04x %04x %04x %01x %02x",
        testSamples[i].WinRdx, testSamples[i].y, testSamples[i].Rid, testSamples[i].Flag, testSamples[i].PeakRltvRdx);
        sw << data << std::endl;
        for (int j = 0; j < LUMA_RGN_DATA_WIN_SIZE; j++)
        {
            sprintf_s(data, "%02x", testSamples[i].data[j]);
            sw << data << std::endl;
        }
    }
    sw.close();
}

void _outputMaskData(char* fileName, double sigma, std::vector<double>& mask_1st, std::vector<double>& mask_2nd,
    double scale, std::vector<int>& nMask_1st, std::vector<int>& nMask_2nd)
{
    std::ofstream sw(fileName);
    char str[250];

    sprintf_s(str, "sigma=%f", sigma);
    sw << str << std::endl;
    sprintf_s(str, "mask_1st:");
    sw << str << std::endl;
    for (int i = 0, i_max = mask_1st.size(); i < i_max; i++)
    {
        if (i < i_max - 1)
            sprintf_s(str, "%f, ", mask_1st[i]);
        else
            sprintf_s(str, "%f", mask_1st[i]);
        sw << str;
    }
    sw << std::endl;

    sprintf_s(str, "mask_2nd:");
    sw << str << std::endl;
    for (int i = 0, i_max = mask_2nd.size(); i < i_max; i++)
    {
        if (i < i_max - 1)
            sprintf_s(str, "%f, ", mask_2nd[i]);
        else
            sprintf_s(str, "%f", mask_2nd[i]);
        sw << str;
    }
    sw << std::endl;

    sw << std::endl;
    sprintf_s(str, "scale=%f", scale);
    sw << str << std::endl;
    sprintf_s(str, "nMask_1st:");
    sw << str << std::endl;
    for (int i = 0, i_max = nMask_1st.size(); i < i_max; i++)
    {
        if (i < i_max - 1)
            sprintf_s(str, "%d, ", nMask_1st[i]);
        else
            sprintf_s(str, "%d", nMask_1st[i]);
        sw << str;
    }
    sw << std::endl;

    sprintf_s(str, "nMask_2nd:");
    sw << str << std::endl;
    for (int i = 0, i_max = nMask_2nd.size(); i < i_max; i++)
    {
        if (i < i_max - 1)
            sprintf_s(str, "%d, ", nMask_2nd[i]);
        else
            sprintf_s(str, "%d", nMask_2nd[i]);
        sw << str;
    }
    sw << std::endl;

    sw.close();
    return;
}

#pragma pack(push, 1)
typedef struct {
    uint16_t file_type;      // 文件类型，'BM'
    uint32_t file_size;      // 文件大小
    uint16_t reserved1;      // 保留字段
    uint16_t reserved2;
    uint32_t offset_data;    // 数据偏移
}BMPFileHeader;

typedef struct {
    uint32_t size;           // 信息头大小（40字节）
    int32_t width;           // 图像宽度
    int32_t height;          // 图像高度（正数表示倒序）
    uint16_t planes;         // 颜色平面数（必须为1）
    uint16_t bit_count;      // 每像素位数（24）
    uint32_t compression;    // 压缩方式（0表示无压缩）
    uint32_t size_image;     // 像素数据大小
    int32_t x_pixels_per_meter;
    int32_t y_pixels_per_meter;
    uint32_t colors_used;
    uint32_t colors_important;
} BMPInfoHeader;
#pragma pack(pop)

void save_bmp(const char* filename, uint8_t* image_data, int width, int height) {
    FILE* file;
    fopen_s(&file, filename, "wb");
    if (!file) {
        std::printf("无法打开文件 %s\n", filename);
        return;
    }

    // 计算每行填充字节数
    int bytes_per_row = width * 3; // 每个像素3字节（BGR）
    int padding = (4 - (bytes_per_row % 4)) % 4;
    int row_size = bytes_per_row + padding;

    // 初始化文件头
    BMPFileHeader file_header = {
        .file_type = 0x4D42, // 'BM'
        .file_size = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader) + row_size * height,
        .reserved1 = 0,
        .reserved2 = 0,
        .offset_data = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader)
    };
    // 初始化信息头
    BMPInfoHeader info_header = {
        .size = sizeof(BMPInfoHeader),
        .width = width,
        .height = height, // 正数表示像素数据从下到上排列
        .planes = 1,
        .bit_count = 24,
        .compression = 0,
        .size_image = (uint32_t)(row_size * height),
        .x_pixels_per_meter = 0,
        .y_pixels_per_meter = 0,
        .colors_used = 0,
        .colors_important = 0
    };

    // 写入头信息
    fwrite(&file_header, 1, sizeof(BMPFileHeader), file);
    fwrite(&info_header, 1, sizeof(BMPInfoHeader), file);

    // 写入像素数据（从最后一行开始）
    uint8_t padding_bytes[3] = { 0, 0, 0 };
    for (int y = height - 1; y >= 0; y--) {
        for (int x = 0; x < width; x++) {
            uint8_t* pixel = image_data + (y * width + x) * 3;
            // 将RGB转为BGR顺序
            fputc(pixel[2], file); // B
            fputc(pixel[1], file); // G
            fputc(pixel[0], file); // R
        }
        fwrite(padding_bytes, 1, padding, file); // 填充
    }

    fclose(file);
}

void save_bmp_2(const char* filename, std::vector<Luma_rgnData>& testSamples) {
    FILE* file;
    fopen_s(&file, filename, "wb");
    if (!file) {
        std::printf("无法打开文件 %s\n", filename);
        return;
    }

    // 计算每行填充字节数
    int bytes_per_row = LUMA_RGN_DATA_WIN_SIZE * 3; // 每个像素3字节（BGR）
    int padding = (4 - (bytes_per_row % 4)) % 4;
    int row_size = bytes_per_row + padding;
    int height = testSamples.size();

    // 初始化文件头
    BMPFileHeader file_header = {
        .file_type = 0x4D42, // 'BM'
        .file_size = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader) + row_size * height,
        .reserved1 = 0,
        .reserved2 = 0,
        .offset_data = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader)
    };
    // 初始化信息头
    BMPInfoHeader info_header = {
        .size = sizeof(BMPInfoHeader),
        .width = LUMA_RGN_DATA_WIN_SIZE,
        .height = height, // 正数表示像素数据从下到上排列
        .planes = 1,
        .bit_count = 24,
        .compression = 0,
        .size_image = (uint32_t)(row_size * height),
        .x_pixels_per_meter = 0,
        .y_pixels_per_meter = 0,
        .colors_used = 0,
        .colors_important = 0
    };

    // 写入头信息
    fwrite(&file_header, 1, sizeof(BMPFileHeader), file);
    fwrite(&info_header, 1, sizeof(BMPInfoHeader), file);

    // 写入像素数据（从最后一行开始）
    uint8_t padding_bytes[3] = { 0, 0, 0 };
    for (int y = height - 1; y >= 0; y--) {
        for (int x = 0; x < LUMA_RGN_DATA_WIN_SIZE; x++) {
            uint8_t pixel = (uint8_t)testSamples[y].data[x];
            // 将RGB转为BGR顺序
            fputc(pixel, file); // B
            fputc(pixel, file); // G
            fputc(pixel, file); // R
        }
        fwrite(padding_bytes, 1, padding, file); // 填充
    }

    fclose(file);
}

#if 0
// 示例使用
int main() {
    const int WIDTH = 640;
    const int HEIGHT = 480;
    uint8_t image[HEIGHT][WIDTH][3]; // 假设图像数据为RGB格式

    // 填充测试数据（红色）
    for (int y = 0; y < HEIGHT; y++) {
        for (int x = 0; x < WIDTH; x++) {
            image[y][x][0] = 255; // R
            image[y][x][1] = 0;   // G
            image[y][x][2] = 0;   // B
        }
    }

    save_bmp("output.bmp", (uint8_t*)image, WIDTH, HEIGHT);
    return 0;
}

void writeBMP(const char* filename, std::vector<Luma_rgnData>& testSamples) 
{
    std::ofstream file(filename, std::ios::out | std::ios::binary);
    if (!file) return;

    // BMP文件头结构体
    struct BMPHeader {
        uint16_t type;           // Magic identifier
        uint32_t size;           // File size in bytes
        uint16_t reserved1;      // Not used
        uint16_t reserved2;      // Not used
        uint32_t offset;         // Start position of pixel data
        uint32_t DIBSize;        // DIB header size
        int32_t width;           // width of bitmap in pixels
        int32_t height;          // width of bitmap in pixels   
        uint16_t planes;         // Number of color planes must be 1   
        uint16_t bits;           // Bits per pixel   
        uint32_t compression;    // Compression type   
        uint32_t imagesize;      // Image size must be 0 for uncompressed images   
        int32_t xresolution;     // Pixels per meter   
        int32_t yresolution;     // Pixels per meter   
        uint32_t ncolor;         // Number of colors   
        uint32_t importantcolor; // Number of important colors   
    } header;

    header.type = 0x4D42; // 'BM' in little endian.
    header.size = 54 + (width * height * 3); // header size + data size (3 bytes per pixel for RGB)
    header.reserved1 = 0;
    header.reserved2 = 0;
    header.offset = 54; // Start of pixel data after the header.
    header.DIBSize = 40; // Size of the DIB header, in bytes.
    header.width = width;
    header.height = height;
    header.planes = 1; // Number of color planes. Must be set to 1.
    header.bits = 24; // Bits per pixel (24 bits for RGB).
    header.compression = 0; // BI_RGB, no compression used.
    header.imagesize = (width * height * 3); // Image size, in bytes. Must be 0 for BI_RGB bitmaps.
    header.xresolution = 0; // Pixels per meter (set to 0 to use default value).
    header.yresolution = 0; // Pixels per meter (set to 0 to use default value).
    header.ncolor = 0; // Number of colors in the color palette. Must be set to 0 for BI_RGB bitmaps.
    header.importantcolor = 0; // All colors are important. Must be set to 0 for BI_RGB bitmaps.

    file.write(reinterpret_cast<const char*>(&header), sizeof(header)); // Write the header to the file.
    file.write(reinterpret_cast<const char*>(data), width * height * 3); // Write the pixel data to the file.
}
#endif
int main() {
#if 0
    // 生成测试信号 (高斯峰位于15.3的位置)
    vector<double> signal(30, 0.0);
    double true_pos = 15.3;
    double sigma = 3.0;
    for (int i = 0; i < signal.size(); ++i) {
        double x = i - true_pos;
        signal[i] = exp(-x * x / (2 * sigma * sigma));
    }
#endif

    double sigmaValue[3] = { 2.0, 3.0, 4.0 };
    double pkValue[10] = {20.0, 40.0, 60.0, 100.0, 200.0, 240.0, 250.0, 260.0, 280.0, 300.0};
    int testId = 0;
    std::vector<Luma_rgnData> testSamples;

    // 参数设置
    double width = 4;
    double sigma = width / 1.5;
    int radius = ceil(5 * sigma);
    std::vector<double> objMask_1st;
    std::vector<double> objMask_2nd;
    _genSubpixMask(
        sigma,
        radius,
        objMask_1st,
        objMask_2nd);

    double scale = 1024.0 * 1024.0 * 8;
    std::vector<int> nObjMask_1st;
    nObjMask_1st.resize(objMask_1st.size());
    std::vector<int> nObjMask_2nd;
    nObjMask_2nd.resize(objMask_2nd.size());
    for (int i = 0; i < objMask_1st.size(); i++)
        nObjMask_1st[i] = (int)(objMask_1st[i] * scale);
    for (int i = 0; i < objMask_2nd.size(); i++)
        nObjMask_2nd[i] = (int)(objMask_2nd[i] * scale);

    for (int sidx = 0; sidx < 3; sidx++)
    {
        double sigma = sigmaValue[sidx];
        for (int loop = 0; loop < 10; loop++)
        {
            std::vector<Luma_rgnData> testSignal = _genTestData(sigma, pkValue[loop]);
            testSamples.insert(testSamples.end(), testSignal.begin(), testSignal.end());
            // 执行亚像素定位
            auto positions = subpixel_localization(testSignal, objMask_1st, objMask_2nd, nObjMask_1st, nObjMask_2nd);
            for (int i = 0, i_max = positions.size(); i < i_max; i++)
            {
                cout << "Test_" << testId << ": ( ";
                for (int j = 0; j < LUMA_RGN_DATA_WIN_SIZE - 1; j++)
                    cout << (int)testSignal[i].data[j] << ",";
                cout << (int)testSignal[i].data[LUMA_RGN_DATA_WIN_SIZE - 1] << " )" ;
                double diff = positions[i].compute_offset - positions[i].true_offset;
                double diff2 = positions[i].compute_offset - positions[i].nCom_offset;
                cout << "  true_offset=" << positions[i].true_offset << " pos=" << positions[i].compute_offset << " nComPos=" << positions[i].nCom_offset << " diff=" << diff << " diff2=" << diff2 << std::endl;
                testId++;
            }
        }
    }

    //文件输出
    char outFile[250];
    sprintf_s(outFile, "F:\\上古\\相机开发\\测试数据\\testSampe.txt");
    _outputTestSample(outFile, testSamples);
    sprintf_s(outFile, "F:\\上古\\相机开发\\测试数据\\testSampe.bmp");
    save_bmp_2(outFile, testSamples);
    sprintf_s(outFile, "F:\\上古\\相机开发\\测试数据\\maskData.txt");
    _outputMaskData(outFile, sigma, objMask_1st, objMask_2nd, scale, nObjMask_1st, nObjMask_2nd);
    std::printf("done!\n");
#if 0
    // 输出结果
    cout << "True position: " << true_pos << endl;
    cout << "Detected positions:\n";
    for (double p : positions) {
        cout << p << " (error: " << fabs(p - true_pos) << ")\n";
    }
#endif
    return 0;
}