/*
 * 自然写互动课堂应用开发SDK软件 V1.0
 * 坐标变换模块 - 点阵笔坐标到屏幕坐标的高精度映射
 *
 * 功能说明：
 * 1. 点阵码坐标解析与标准化（Anoto编码 → 物理坐标mm）
 * 2. 仿射变换矩阵计算（四角标定点 → 变换参数）
 * 3. 物理坐标到屏幕像素坐标的实时映射
 * 4. 多页面坐标空间管理（不同纸张/不同页面独立坐标系）
 * 5. 畸变校正（镜头畸变、纸张弯曲补偿）
 */

#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

/* ========== 数据结构定义 ========== */

/* 二维点（浮点精度） */
typedef struct {
    double x;      /* X坐标 */
    double y;      /* Y坐标 */
} Point2D;

/* 仿射变换矩阵 3x3（齐次坐标） */
typedef struct {
    double m[3][3];    /* 变换矩阵元素 */
} AffineMatrix;

/* 坐标空间描述 */
typedef struct {
    unsigned int page_id;          /* 页面唯一ID */
    unsigned int section_id;       /* 区段ID（Anoto编码中的section） */
    unsigned int owner_id;         /* 拥有者ID（Anoto编码） */
    double physical_width_mm;      /* 纸张物理宽度（毫米） */
    double physical_height_mm;     /* 纸张物理高度（毫米） */
    int screen_width_px;           /* 对应屏幕区域宽度（像素） */
    int screen_height_px;          /* 对应屏幕区域高度（像素） */
    AffineMatrix transform;        /* 标定后的变换矩阵 */
    int is_calibrated;             /* 是否已完成标定 */
} CoordinateSpace;

/* 标定点对（物理坐标 ↔ 屏幕坐标） */
typedef struct {
    Point2D physical;      /* 物理坐标（mm） */
    Point2D screen;        /* 屏幕坐标（px） */
} CalibrationPair;

/* 畸变校正参数（Brown-Conrady模型简化版） */
typedef struct {
    double k1;             /* 径向畸变系数1 */
    double k2;             /* 径向畸变系数2 */
    double p1;             /* 切向畸变系数1 */
    double p2;             /* 切向畸变系数2 */
    double cx;             /* 畸变中心X */
    double cy;             /* 畸变中心Y */
} DistortionParams;

/* 坐标变换管理器 */
typedef struct {
    CoordinateSpace spaces[64];      /* 最多支持64个坐标空间 */
    int space_count;                 /* 当前已注册的空间数 */
    DistortionParams distortion;     /* 全局畸变校正参数 */
    int distortion_enabled;          /* 是否启用畸变校正 */
    double dpi_resolution;           /* 点阵笔DPI分辨率（通常为300或600） */
} CoordinateManager;

/* 全局坐标管理器实例 */
static CoordinateManager g_coord_manager;

/* ========== Anoto点阵码坐标解析 ========== */

/*
 * 将Anoto点阵码原始编码转换为物理坐标（毫米）
 * 点阵笔采集到的原始数据是基于Anoto编码系统的逻辑坐标
 * 需要根据DPI分辨率转换为实际的物理距离
 *
 * @param raw_x        点阵码原始X坐标值
 * @param raw_y        点阵码原始Y坐标值
 * @param section_id   Anoto编码的section标识
 * @param out_physical  输出的物理坐标（mm）
 * @return 0成功, -1参数错误
 */
int anoto_to_physical(unsigned int raw_x, unsigned int raw_y,
                       unsigned int section_id, Point2D *out_physical) {
    if (out_physical == NULL) {
        return -1;
    }

    /* DPI到毫米的转换因子：25.4mm / DPI */
    double dpi = g_coord_manager.dpi_resolution;
    if (dpi < 1.0) {
        dpi = 300.0;   /* 默认300 DPI */
    }
    double dots_to_mm = 25.4 / dpi;

    /* Anoto编码的原始坐标直接乘以转换因子得到物理坐标 */
    out_physical->x = (double)raw_x * dots_to_mm;
    out_physical->y = (double)raw_y * dots_to_mm;

    return 0;
}

/*
 * 解析7字节紧凑坐标编码
 * 点阵笔通过BLE传输时使用7字节紧凑格式：
 *   字节0-1: X坐标高16位
 *   字节2-3: Y坐标高16位
 *   字节4:   X低4位 | Y低4位
 *   字节5:   压力值高8位
 *   字节6:   压力值低8位 | 标志位
 */
int decode_compact_coordinate(const unsigned char *data, int data_len,
                               unsigned int *out_x, unsigned int *out_y,
                               unsigned int *out_pressure) {
    if (data == NULL || data_len < 7) {
        return -1;
    }

    /* 解析X坐标（20位精度） */
    unsigned int x_high = ((unsigned int)data[0] << 8) | data[1];
    unsigned int x_low = (data[4] >> 4) & 0x0F;
    *out_x = (x_high << 4) | x_low;

    /* 解析Y坐标（20位精度） */
    unsigned int y_high = ((unsigned int)data[2] << 8) | data[3];
    unsigned int y_low = data[4] & 0x0F;
    *out_y = (y_high << 4) | y_low;

    /* 解析压力值（12位精度，0-4095） */
    unsigned int p_high = data[5];
    unsigned int p_low = (data[6] >> 4) & 0x0F;
    *out_pressure = (p_high << 4) | p_low;

    return 0;
}

/* ========== 仿射变换矩阵计算 ========== */

/*
 * 初始化为单位矩阵
 */
void matrix_identity(AffineMatrix *mat) {
    memset(mat->m, 0, sizeof(mat->m));
    mat->m[0][0] = 1.0;
    mat->m[1][1] = 1.0;
    mat->m[2][2] = 1.0;
}

/*
 * 矩阵乘法 result = a * b
 */
void matrix_multiply(const AffineMatrix *a, const AffineMatrix *b,
                      AffineMatrix *result) {
    AffineMatrix tmp;
    int i, j, k;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            tmp.m[i][j] = 0.0;
            for (k = 0; k < 3; k++) {
                tmp.m[i][j] += a->m[i][k] * b->m[k][j];
            }
        }
    }
    memcpy(result->m, tmp.m, sizeof(tmp.m));
}

/*
 * 使用最小二乘法从标定点对计算仿射变换矩阵
 * 至少需要3个不共线的标定点对
 * 使用正规方程法求解超定线性方程组
 *
 * @param pairs       标定点对数组
 * @param pair_count  标定点对数量（≥3）
 * @param out_matrix  输出的仿射变换矩阵
 * @return 0成功, -1参数不足, -2矩阵奇异
 */
int compute_affine_transform(const CalibrationPair *pairs, int pair_count,
                              AffineMatrix *out_matrix) {
    if (pairs == NULL || pair_count < 3 || out_matrix == NULL) {
        return -1;
    }

    /*
     * 仿射变换方程：
     * screen_x = a11 * phys_x + a12 * phys_y + a13
     * screen_y = a21 * phys_x + a22 * phys_y + a23
     *
     * 构建 ATA * x = ATb 正规方程
     * A矩阵每行: [phys_x, phys_y, 1]
     */
    double ATA[3][3] = {{0}};
    double ATb_x[3] = {0};
    double ATb_y[3] = {0};

    int i;
    for (i = 0; i < pair_count; i++) {
        double px = pairs[i].physical.x;
        double py = pairs[i].physical.y;
        double sx = pairs[i].screen.x;
        double sy = pairs[i].screen.y;

        /* 累加 ATA */
        ATA[0][0] += px * px;
        ATA[0][1] += px * py;
        ATA[0][2] += px;
        ATA[1][0] += py * px;
        ATA[1][1] += py * py;
        ATA[1][2] += py;
        ATA[2][0] += px;
        ATA[2][1] += py;
        ATA[2][2] += 1.0;

        /* 累加 ATb */
        ATb_x[0] += px * sx;
        ATb_x[1] += py * sx;
        ATb_x[2] += sx;

        ATb_y[0] += px * sy;
        ATb_y[1] += py * sy;
        ATb_y[2] += sy;
    }

    /* 高斯消元法求解3x3线性方程组 */
    /* 先求解 screen_x 的系数 [a11, a12, a13] */
    double aug_x[3][4];
    double aug_y[3][4];
    int j, k;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            aug_x[i][j] = ATA[i][j];
            aug_y[i][j] = ATA[i][j];
        }
        aug_x[i][3] = ATb_x[i];
        aug_y[i][3] = ATb_y[i];
    }

    /* 高斯消元（部分主元选取） */
    for (k = 0; k < 3; k++) {
        /* 找主元 */
        int max_row = k;
        double max_val = fabs(aug_x[k][k]);
        for (i = k + 1; i < 3; i++) {
            if (fabs(aug_x[i][k]) > max_val) {
                max_val = fabs(aug_x[i][k]);
                max_row = i;
            }
        }
        if (max_val < 1e-12) {
            return -2;   /* 矩阵奇异，标定点可能共线 */
        }
        /* 交换行 */
        if (max_row != k) {
            for (j = 0; j < 4; j++) {
                double tmp = aug_x[k][j];
                aug_x[k][j] = aug_x[max_row][j];
                aug_x[max_row][j] = tmp;
                tmp = aug_y[k][j];
                aug_y[k][j] = aug_y[max_row][j];
                aug_y[max_row][j] = tmp;
            }
        }
        /* 消元 */
        for (i = k + 1; i < 3; i++) {
            double factor_x = aug_x[i][k] / aug_x[k][k];
            double factor_y = aug_y[i][k] / aug_y[k][k];
            for (j = k; j < 4; j++) {
                aug_x[i][j] -= factor_x * aug_x[k][j];
                aug_y[i][j] -= factor_y * aug_y[k][j];
            }
        }
    }

    /* 回代求解 */
    double sol_x[3], sol_y[3];
    for (i = 2; i >= 0; i--) {
        sol_x[i] = aug_x[i][3];
        sol_y[i] = aug_y[i][3];
        for (j = i + 1; j < 3; j++) {
            sol_x[i] -= aug_x[i][j] * sol_x[j];
            sol_y[i] -= aug_y[i][j] * sol_y[j];
        }
        sol_x[i] /= aug_x[i][i];
        sol_y[i] /= aug_y[i][i];
    }

    /* 填充仿射变换矩阵 */
    out_matrix->m[0][0] = sol_x[0];   /* a11 */
    out_matrix->m[0][1] = sol_x[1];   /* a12 */
    out_matrix->m[0][2] = sol_x[2];   /* a13（平移X） */
    out_matrix->m[1][0] = sol_y[0];   /* a21 */
    out_matrix->m[1][1] = sol_y[1];   /* a22 */
    out_matrix->m[1][2] = sol_y[2];   /* a23（平移Y） */
    out_matrix->m[2][0] = 0.0;
    out_matrix->m[2][1] = 0.0;
    out_matrix->m[2][2] = 1.0;

    return 0;
}

/* ========== 坐标空间管理 ========== */

/*
 * 初始化坐标变换管理器
 * @param dpi  点阵笔的DPI分辨率（常见值：300, 600）
 */
void coordinate_manager_init(double dpi) {
    memset(&g_coord_manager, 0, sizeof(g_coord_manager));
    g_coord_manager.dpi_resolution = dpi;
    g_coord_manager.distortion_enabled = 0;
}

/*
 * 注册一个新的坐标空间（对应一个页面/纸张）
 * 在使用特定页面前需先注册其坐标空间参数
 *
 * @param page_id     页面唯一标识
 * @param section_id  Anoto section编号
 * @param width_mm    纸张物理宽度
 * @param height_mm   纸张物理高度
 * @param screen_w    对应屏幕宽度像素
 * @param screen_h    对应屏幕高度像素
 * @return 空间索引, -1失败
 */
int register_coordinate_space(unsigned int page_id, unsigned int section_id,
                               double width_mm, double height_mm,
                               int screen_w, int screen_h) {
    if (g_coord_manager.space_count >= 64) {
        return -1;   /* 空间已满 */
    }

    int idx = g_coord_manager.space_count;
    CoordinateSpace *space = &g_coord_manager.spaces[idx];
    space->page_id = page_id;
    space->section_id = section_id;
    space->physical_width_mm = width_mm;
    space->physical_height_mm = height_mm;
    space->screen_width_px = screen_w;
    space->screen_height_px = screen_h;
    space->is_calibrated = 0;
    matrix_identity(&space->transform);

    g_coord_manager.space_count++;
    return idx;
}

/*
 * 对指定坐标空间执行标定
 * 使用用户提供的标定点对计算仿射变换矩阵
 */
int calibrate_space(int space_index, const CalibrationPair *pairs,
                     int pair_count) {
    if (space_index < 0 || space_index >= g_coord_manager.space_count) {
        return -1;
    }

    CoordinateSpace *space = &g_coord_manager.spaces[space_index];
    int ret = compute_affine_transform(pairs, pair_count, &space->transform);
    if (ret == 0) {
        space->is_calibrated = 1;
    }
    return ret;
}

/*
 * 使用默认缩放（无旋转无畸变）进行快速标定
 * 适用于标准A4纸张等无需精确标定的场景
 */
int calibrate_space_default(int space_index) {
    if (space_index < 0 || space_index >= g_coord_manager.space_count) {
        return -1;
    }

    CoordinateSpace *space = &g_coord_manager.spaces[space_index];
    matrix_identity(&space->transform);

    /* 简单线性缩放：物理mm → 屏幕px */
    double scale_x = (double)space->screen_width_px / space->physical_width_mm;
    double scale_y = (double)space->screen_height_px / space->physical_height_mm;

    space->transform.m[0][0] = scale_x;
    space->transform.m[1][1] = scale_y;
    space->is_calibrated = 1;

    return 0;
}

/* ========== 畸变校正 ========== */

/*
 * 设置畸变校正参数
 * 用于补偿摄像头镜头的径向和切向畸变
 */
void set_distortion_params(double k1, double k2, double p1, double p2,
                            double cx, double cy) {
    g_coord_manager.distortion.k1 = k1;
    g_coord_manager.distortion.k2 = k2;
    g_coord_manager.distortion.p1 = p1;
    g_coord_manager.distortion.p2 = p2;
    g_coord_manager.distortion.cx = cx;
    g_coord_manager.distortion.cy = cy;
    g_coord_manager.distortion_enabled = 1;
}

/*
 * 对物理坐标应用畸变校正（去畸变）
 * 使用Brown-Conrady模型的简化版本
 *
 * @param in    输入的物理坐标
 * @param out   校正后的物理坐标
 */
void apply_distortion_correction(const Point2D *in, Point2D *out) {
    if (!g_coord_manager.distortion_enabled) {
        out->x = in->x;
        out->y = in->y;
        return;
    }

    DistortionParams *d = &g_coord_manager.distortion;

    /* 以畸变中心为原点 */
    double dx = in->x - d->cx;
    double dy = in->y - d->cy;
    double r2 = dx * dx + dy * dy;
    double r4 = r2 * r2;

    /* 径向畸变校正 */
    double radial = 1.0 + d->k1 * r2 + d->k2 * r4;

    /* 切向畸变校正 */
    double tang_x = 2.0 * d->p1 * dx * dy + d->p2 * (r2 + 2.0 * dx * dx);
    double tang_y = d->p1 * (r2 + 2.0 * dy * dy) + 2.0 * d->p2 * dx * dy;

    out->x = d->cx + dx * radial + tang_x;
    out->y = d->cy + dy * radial + tang_y;
}

/* ========== 坐标变换核心接口 ========== */

/*
 * 根据page_id查找对应的坐标空间索引
 */
int find_space_by_page(unsigned int page_id) {
    int i;
    for (i = 0; i < g_coord_manager.space_count; i++) {
        if (g_coord_manager.spaces[i].page_id == page_id) {
            return i;
        }
    }
    return -1;
}

/*
 * 完整坐标变换流水线：原始点阵码坐标 → 屏幕像素坐标
 *
 * 处理步骤：
 * 1. Anoto编码 → 物理坐标（mm）
 * 2. 畸变校正（如果启用）
 * 3. 仿射变换 → 屏幕坐标（px）
 * 4. 边界裁剪（确保不超出屏幕范围）
 *
 * @param raw_x       原始X坐标
 * @param raw_y       原始Y坐标
 * @param page_id     页面ID
 * @param out_screen  输出屏幕坐标
 * @return 0成功, -1未找到坐标空间, -2未标定
 */
int transform_coordinate(unsigned int raw_x, unsigned int raw_y,
                          unsigned int page_id, Point2D *out_screen) {
    if (out_screen == NULL) {
        return -1;
    }

    /* 查找坐标空间 */
    int idx = find_space_by_page(page_id);
    if (idx < 0) {
        return -1;
    }

    CoordinateSpace *space = &g_coord_manager.spaces[idx];
    if (!space->is_calibrated) {
        return -2;
    }

    /* 步骤1：原始坐标 → 物理坐标 */
    Point2D physical;
    anoto_to_physical(raw_x, raw_y, space->section_id, &physical);

    /* 步骤2：畸变校正 */
    Point2D corrected;
    apply_distortion_correction(&physical, &corrected);

    /* 步骤3：仿射变换 → 屏幕坐标 */
    AffineMatrix *mat = &space->transform;
    out_screen->x = mat->m[0][0] * corrected.x
                   + mat->m[0][1] * corrected.y
                   + mat->m[0][2];
    out_screen->y = mat->m[1][0] * corrected.x
                   + mat->m[1][1] * corrected.y
                   + mat->m[1][2];

    /* 步骤4：边界裁剪 */
    if (out_screen->x < 0.0) out_screen->x = 0.0;
    if (out_screen->y < 0.0) out_screen->y = 0.0;
    if (out_screen->x > (double)space->screen_width_px) {
        out_screen->x = (double)space->screen_width_px;
    }
    if (out_screen->y > (double)space->screen_height_px) {
        out_screen->y = (double)space->screen_height_px;
    }

    return 0;
}

/*
 * 批量坐标变换（优化版，避免重复查找坐标空间）
 * 适用于一次性转换整条笔画的所有采样点
 *
 * @param raw_points   原始坐标数组，每组2个unsigned int (x, y)
 * @param point_count  坐标点数量
 * @param page_id      页面ID
 * @param out_screen   输出屏幕坐标数组（调用者负责分配内存）
 * @return 成功转换的点数
 */
int transform_batch(const unsigned int *raw_points, int point_count,
                     unsigned int page_id, Point2D *out_screen) {
    int idx = find_space_by_page(page_id);
    if (idx < 0 || out_screen == NULL) {
        return 0;
    }

    CoordinateSpace *space = &g_coord_manager.spaces[idx];
    if (!space->is_calibrated) {
        return 0;
    }

    double dpi = g_coord_manager.dpi_resolution;
    if (dpi < 1.0) dpi = 300.0;
    double dots_to_mm = 25.4 / dpi;

    AffineMatrix *mat = &space->transform;
    int converted = 0;
    int i;

    for (i = 0; i < point_count; i++) {
        /* 直接内联计算，减少函数调用开销 */
        double px = (double)raw_points[i * 2] * dots_to_mm;
        double py = (double)raw_points[i * 2 + 1] * dots_to_mm;

        /* 畸变校正（内联） */
        if (g_coord_manager.distortion_enabled) {
            DistortionParams *d = &g_coord_manager.distortion;
            double dx = px - d->cx;
            double dy = py - d->cy;
            double r2 = dx * dx + dy * dy;
            double radial = 1.0 + d->k1 * r2 + d->k2 * r2 * r2;
            px = d->cx + dx * radial + 2.0 * d->p1 * dx * dy
                 + d->p2 * (r2 + 2.0 * dx * dx);
            py = d->cy + dy * radial + d->p1 * (r2 + 2.0 * dy * dy)
                 + 2.0 * d->p2 * dx * dy;
        }

        /* 仿射变换 */
        double sx = mat->m[0][0] * px + mat->m[0][1] * py + mat->m[0][2];
        double sy = mat->m[1][0] * px + mat->m[1][1] * py + mat->m[1][2];

        /* 边界裁剪 */
        if (sx < 0.0) sx = 0.0;
        if (sy < 0.0) sy = 0.0;
        if (sx > (double)space->screen_width_px) sx = (double)space->screen_width_px;
        if (sy > (double)space->screen_height_px) sy = (double)space->screen_height_px;

        out_screen[i].x = sx;
        out_screen[i].y = sy;
        converted++;
    }

    return converted;
}

/*
 * 反向变换：屏幕坐标 → 物理坐标
 * 用于在屏幕上点击后反推纸面物理位置
 * 需要计算仿射变换矩阵的逆矩阵
 */
int inverse_transform(double screen_x, double screen_y,
                       unsigned int page_id, Point2D *out_physical) {
    int idx = find_space_by_page(page_id);
    if (idx < 0 || out_physical == NULL) {
        return -1;
    }

    CoordinateSpace *space = &g_coord_manager.spaces[idx];
    AffineMatrix *mat = &space->transform;

    /* 计算2x2子矩阵的行列式 */
    double det = mat->m[0][0] * mat->m[1][1] - mat->m[0][1] * mat->m[1][0];
    if (fabs(det) < 1e-12) {
        return -2;   /* 矩阵不可逆 */
    }

    double inv_det = 1.0 / det;

    /* 减去平移分量 */
    double tx = screen_x - mat->m[0][2];
    double ty = screen_y - mat->m[1][2];

    /* 应用逆矩阵 */
    out_physical->x = inv_det * (mat->m[1][1] * tx - mat->m[0][1] * ty);
    out_physical->y = inv_det * (mat->m[0][0] * ty - mat->m[1][0] * tx);

    return 0;
}
