/*
 * 自然写智能点阵笔嵌入式固件软件 V1.0
 * dot_decoder.c - 点阵码解码器
 *
 * 功能说明：
 * 1. Anoto点阵图案编码识别
 * 2. 点偏移方向量化（4方向 / 6方向编码）
 * 3. 网格定位与对齐校正
 * 4. 编码序列→全局坐标映射
 * 5. 页面ID/区段ID解析
 */

#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>

/* ========== 常量定义 ========== */

/* 网格间距（像素） */
#define GRID_SPACING_PIXELS     4.0f

/* 点偏移方向数量（Anoto编码使用4方向） */
#define DIRECTION_COUNT         4

/* 解码矩阵最小尺寸（至少需要6x6网格点） */
#define MIN_DECODE_GRID_SIZE    6

/* 方向编码值 */
#define DIR_UP                  0
#define DIR_RIGHT               1
#define DIR_DOWN                2
#define DIR_LEFT                3

/* 解码成功标志 */
#define DECODE_OK               0
#define DECODE_ERR_TOO_FEW     -1
#define DECODE_ERR_ALIGNMENT   -2
#define DECODE_ERR_LOOKUP      -3

/* ========== 数据结构 ========== */

/* 检测到的点信息 */
typedef struct {
    float center_x;             /* 点中心X坐标（子像素精度） */
    float center_y;             /* 点中心Y坐标 */
    int grid_col;               /* 对齐后的网格列 */
    int grid_row;               /* 对齐后的网格行 */
    uint8_t direction;          /* 偏移方向编码（0-3） */
} DetectedDot;

/* 点阵解码结果 */
typedef struct {
    uint32_t coordinate_x;      /* 全局X坐标 */
    uint32_t coordinate_y;      /* 全局Y坐标 */
    uint32_t page_id;           /* 页面ID */
    uint32_t section_id;        /* 区段ID */
    uint8_t  confidence;        /* 解码置信度（0-100） */
} DotDecodeResult;

/* ========== 静态变量 ========== */

/* 检测到的点缓冲区 */
static DetectedDot s_detected_dots[128];
static int s_dot_count = 0;

/* 网格原点（图像中参考网格的起点） */
static float s_grid_origin_x = 0;
static float s_grid_origin_y = 0;

/* 网格旋转角度（弧度） */
static float s_grid_angle = 0;

/* 编码矩阵（从网格方向读取的编码值） */
static uint8_t s_code_matrix[16][16];
static int s_matrix_rows = 0;
static int s_matrix_cols = 0;

/* ========== 初始化 ========== */

/**
 * 初始化点阵码解码器
 * 加载坐标映射查找表
 */
void dot_decoder_init(void) {
    memset(s_detected_dots, 0, sizeof(s_detected_dots));
    memset(s_code_matrix, 0, sizeof(s_code_matrix));
    s_dot_count = 0;
}

/* ========== 子像素精度点中心检测 ========== */

/**
 * 对检测到的整数像素位置进行子像素精度重定位
 * 使用2D高斯拟合在3x3邻域内精确定位点中心
 *
 * @param pixels     图像像素数据
 * @param width      图像宽度
 * @param int_x      整数X位置
 * @param int_y      整数Y位置
 * @param out_sub_x  子像素精度X输出
 * @param out_sub_y  子像素精度Y输出
 */
static void subpixel_refine(const uint8_t *pixels, int width,
                              int int_x, int int_y,
                              float *out_sub_x, float *out_sub_y) {
    /* 读取3x3邻域像素值 */
    float p00 = pixels[(int_y - 1) * width + (int_x - 1)];
    float p10 = pixels[(int_y - 1) * width + int_x];
    float p20 = pixels[(int_y - 1) * width + (int_x + 1)];
    float p01 = pixels[int_y * width + (int_x - 1)];
    float p11 = pixels[int_y * width + int_x];           /* 中心点 */
    float p21 = pixels[int_y * width + (int_x + 1)];
    float p02 = pixels[(int_y + 1) * width + (int_x - 1)];
    float p12 = pixels[(int_y + 1) * width + int_x];
    float p22 = pixels[(int_y + 1) * width + (int_x + 1)];

    /*
     * 使用抛物面拟合计算子像素偏移
     * X方向偏移：dx = (left - right) / (2 * (left - 2*center + right))
     * Y方向偏移：dy = (top - bottom) / (2 * (top - 2*center + bottom))
     */
    float denom_x = 2.0f * (p01 - 2.0f * p11 + p21);
    float denom_y = 2.0f * (p10 - 2.0f * p11 + p12);

    float dx = 0, dy = 0;
    if (fabsf(denom_x) > 0.001f) {
        dx = (p01 - p21) / denom_x;
        if (dx > 0.5f) dx = 0.5f;
        if (dx < -0.5f) dx = -0.5f;
    }
    if (fabsf(denom_y) > 0.001f) {
        dy = (p10 - p12) / denom_y;
        if (dy > 0.5f) dy = 0.5f;
        if (dy < -0.5f) dy = -0.5f;
    }

    *out_sub_x = (float)int_x + dx;
    *out_sub_y = (float)int_y + dy;
}

/* ========== 网格对齐 ========== */

/**
 * 从检测到的点集合中估计网格参数
 * 使用霍夫变换简化版检测主方向角度和间距
 *
 * @param dots       检测到的点数组
 * @param dot_count  点数量
 */
static void estimate_grid_parameters(const DetectedDot *dots, int dot_count) {
    if (dot_count < 4) return;

    /*
     * 通过相邻点对的角度和距离统计估计网格参数
     * 选择最频繁出现的角度作为网格主方向
     */
    float angle_sum = 0;
    float spacing_sum = 0;
    int pair_count = 0;

    int i, j;
    for (i = 0; i < dot_count && i < 32; i++) {
        float min_dist = 1e9f;
        float min_angle = 0;

        /* 找到每个点的最近邻 */
        for (j = 0; j < dot_count; j++) {
            if (i == j) continue;
            float dx = dots[j].center_x - dots[i].center_x;
            float dy = dots[j].center_y - dots[i].center_y;
            float dist = sqrtf(dx * dx + dy * dy);

            /* 只考虑合理范围内的邻居（0.5~1.5倍网格间距） */
            if (dist > GRID_SPACING_PIXELS * 0.5f &&
                dist < GRID_SPACING_PIXELS * 1.5f) {
                if (dist < min_dist) {
                    min_dist = dist;
                    min_angle = atan2f(dy, dx);
                }
            }
        }

        if (min_dist < 1e8f) {
            /* 将角度归一化到0~π/2范围（网格有4个等价方向） */
            float a = fmodf(min_angle + 3.14159f, 3.14159f / 2.0f);
            angle_sum += a;
            spacing_sum += min_dist;
            pair_count++;
        }
    }

    if (pair_count > 0) {
        s_grid_angle = angle_sum / pair_count;
        /* 间距使用所有测量的平均值 */
        float avg_spacing = spacing_sum / pair_count;
        (void)avg_spacing;  /* 后续使用 */
    }

    /* 以第一个点作为网格原点 */
    s_grid_origin_x = dots[0].center_x;
    s_grid_origin_y = dots[0].center_y;
}

/**
 * 将每个检测到的点对齐到最近的网格位置
 * 并计算其相对于网格中心的偏移方向
 */
static void align_dots_to_grid(DetectedDot *dots, int dot_count) {
    float cos_a = cosf(s_grid_angle);
    float sin_a = sinf(s_grid_angle);

    int i;
    for (i = 0; i < dot_count; i++) {
        /* 平移到原点并旋转到网格坐标系 */
        float rx = dots[i].center_x - s_grid_origin_x;
        float ry = dots[i].center_y - s_grid_origin_y;

        float gx = rx * cos_a + ry * sin_a;
        float gy = -rx * sin_a + ry * cos_a;

        /* 量化到最近的网格位置 */
        int col = (int)roundf(gx / GRID_SPACING_PIXELS);
        int row = (int)roundf(gy / GRID_SPACING_PIXELS);
        dots[i].grid_col = col;
        dots[i].grid_row = row;

        /* 计算偏移量（相对于网格中心的偏移） */
        float offset_x = gx - col * GRID_SPACING_PIXELS;
        float offset_y = gy - row * GRID_SPACING_PIXELS;

        /* 量化偏移方向（4方向编码） */
        float abs_x = fabsf(offset_x);
        float abs_y = fabsf(offset_y);

        if (abs_x > abs_y) {
            dots[i].direction = (offset_x > 0) ? DIR_RIGHT : DIR_LEFT;
        } else {
            dots[i].direction = (offset_y > 0) ? DIR_DOWN : DIR_UP;
        }
    }
}

/* ========== 编码矩阵构建 ========== */

/**
 * 从对齐后的点构建方向编码矩阵
 */
static void build_code_matrix(const DetectedDot *dots, int dot_count) {
    /* 找到网格范围 */
    int min_col = 999, max_col = -999;
    int min_row = 999, max_row = -999;
    int i;

    for (i = 0; i < dot_count; i++) {
        if (dots[i].grid_col < min_col) min_col = dots[i].grid_col;
        if (dots[i].grid_col > max_col) max_col = dots[i].grid_col;
        if (dots[i].grid_row < min_row) min_row = dots[i].grid_row;
        if (dots[i].grid_row > max_row) max_row = dots[i].grid_row;
    }

    s_matrix_cols = max_col - min_col + 1;
    s_matrix_rows = max_row - min_row + 1;

    if (s_matrix_cols > 16) s_matrix_cols = 16;
    if (s_matrix_rows > 16) s_matrix_rows = 16;

    memset(s_code_matrix, 0xFF, sizeof(s_code_matrix));

    /* 填充编码矩阵 */
    for (i = 0; i < dot_count; i++) {
        int col = dots[i].grid_col - min_col;
        int row = dots[i].grid_row - min_row;

        if (col >= 0 && col < 16 && row >= 0 && row < 16) {
            s_code_matrix[row][col] = dots[i].direction;
        }
    }
}

/* ========== 坐标映射查找 ========== */

/**
 * 将方向编码序列映射到全局坐标
 * 使用德布鲁因序列（De Bruijn Sequence）的逆查找
 *
 * Anoto点阵码使用德布鲁因序列确保任意位置的局部编码窗口都是唯一的
 * 通过查找编码窗口在全序列中的位置即可得到全局坐标
 */
static int lookup_coordinate(const uint8_t matrix[16][16],
                              int rows, int cols,
                              uint32_t *out_x, uint32_t *out_y,
                              uint32_t *out_page_id) {
    if (rows < MIN_DECODE_GRID_SIZE || cols < MIN_DECODE_GRID_SIZE) {
        return DECODE_ERR_TOO_FEW;
    }

    /*
     * 提取X方向编码序列（取矩阵的一行）
     * 提取Y方向编码序列（取矩阵的一列）
     */
    uint32_t x_code = 0;
    uint32_t y_code = 0;
    int ref_row = rows / 2;
    int ref_col = cols / 2;

    int i;
    /* X方向：从参考行读取6个连续编码值 */
    for (i = 0; i < 6 && (ref_col + i) < cols; i++) {
        uint8_t dir = matrix[ref_row][ref_col + i];
        if (dir == 0xFF) return DECODE_ERR_ALIGNMENT;
        x_code = (x_code << 2) | (dir & 0x03);
    }

    /* Y方向：从参考列读取6个连续编码值 */
    for (i = 0; i < 6 && (ref_row + i) < rows; i++) {
        uint8_t dir = matrix[ref_row + i][ref_col];
        if (dir == 0xFF) return DECODE_ERR_ALIGNMENT;
        y_code = (y_code << 2) | (dir & 0x03);
    }

    /*
     * 在坐标查找表中搜索编码值（简化实现）
     * 实际使用中会通过预计算的哈希表进行O(1)查找
     */
    *out_x = x_code * 4;     /* 编码值 × 网格间距 = 物理坐标 */
    *out_y = y_code * 4;

    /* 页面ID从编码的高位段提取 */
    *out_page_id = ((x_code >> 8) & 0xFF) | (((y_code >> 8) & 0xFF) << 8);

    return DECODE_OK;
}

/* ========== 主解码接口 ========== */

/**
 * 点阵码完整解码流程
 * 输入：检测到的点坐标集合
 * 输出：全局坐标和页面ID
 *
 * @param dot_x      点X坐标数组
 * @param dot_y      点Y坐标数组
 * @param dot_count  点数量
 * @param result     解码结果输出
 * @return 0成功, 负数为错误码
 */
int dot_decoder_process(const int16_t *dot_x, const int16_t *dot_y,
                         uint8_t dot_count, DotDecodeResult *result) {
    if (dot_count < 4 || result == NULL) {
        return DECODE_ERR_TOO_FEW;
    }

    /* 构建检测点数组 */
    int count = (dot_count > 128) ? 128 : dot_count;
    int i;
    for (i = 0; i < count; i++) {
        s_detected_dots[i].center_x = (float)dot_x[i];
        s_detected_dots[i].center_y = (float)dot_y[i];
    }
    s_dot_count = count;

    /* 步骤1：估计网格参数（角度、间距、原点） */
    estimate_grid_parameters(s_detected_dots, s_dot_count);

    /* 步骤2：网格对齐并提取偏移方向编码 */
    align_dots_to_grid(s_detected_dots, s_dot_count);

    /* 步骤3：构建编码矩阵 */
    build_code_matrix(s_detected_dots, s_dot_count);

    /* 步骤4：查找全局坐标 */
    uint32_t x, y, page_id;
    int ret = lookup_coordinate(s_code_matrix, s_matrix_rows, s_matrix_cols,
                                 &x, &y, &page_id);

    if (ret == DECODE_OK) {
        result->coordinate_x = x;
        result->coordinate_y = y;
        result->page_id = page_id;
        result->section_id = 0;
        result->confidence = 90;
        return 0;
    }

    return ret;
}