/*
 * 自然写智能点阵笔嵌入式固件软件 V1.0
 * coordinate_task.c - 坐标计算任务
 *
 * 功能说明：
 * 1. 从图像帧中解码Anoto点阵图案
 * 2. 计算笔尖在纸面的物理坐标
 * 3. 坐标滤波与去抖（卡尔曼滤波）
 * 4. 坐标打包为BLE传输格式
 */

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

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

#include "dot_decoder.h"

/* ========== 坐标数据包结构 ========== */

typedef struct {
    uint32_t raw_x;                 /* 原始X坐标（20位精度） */
    uint32_t raw_y;                 /* 原始Y坐标 */
    uint16_t pressure;              /* 压力值（12位） */
    uint32_t timestamp_ms;          /* 时间戳 */
    uint32_t page_id;               /* 页面ID */
    uint8_t  pen_state;             /* 笔状态：0=书写中, 1=笔落下, 2=笔抬起 */
} CoordinatePacket;

/* ========== 卡尔曼滤波器 ========== */

typedef struct {
    float x_estimate;               /* X状态估计值 */
    float y_estimate;               /* Y状态估计值 */
    float x_error;                  /* X估计误差协方差 */
    float y_error;                  /* Y估计误差协方差 */
    float process_noise;            /* 过程噪声 Q */
    float measurement_noise;        /* 测量噪声 R */
    bool initialized;               /* 是否已初始化 */
} KalmanFilter2D;

/* ========== 外部引用 ========== */

extern QueueHandle_t g_image_data_queue;
extern QueueHandle_t g_coordinate_queue;

/* ========== 图像帧元数据结构（与image_capture_task一致） ========== */

typedef struct {
    uint8_t *pixel_buffer;
    uint32_t frame_id;
    uint32_t timestamp_ms;
    uint8_t quality_score;
    uint8_t exposure_value;
    uint16_t pressure_raw;
} ImageFrameMetadata;

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

/* 卡尔曼滤波器实例 */
static KalmanFilter2D s_kalman;

/* 上一次有效坐标（用于抖动检测） */
static float s_last_valid_x = 0;
static float s_last_valid_y = 0;

/* 点阵码解码工作缓冲区 */
static uint8_t s_decode_buffer[128];

/* 统计信息 */
static uint32_t s_total_decoded = 0;
static uint32_t s_decode_failures = 0;

/* ========== 卡尔曼滤波实现 ========== */

/**
 * 初始化卡尔曼滤波器
 * @param kf     滤波器实例
 * @param q      过程噪声（越大跟踪越快，噪声越多）
 * @param r      测量噪声（越大滤波越强，延迟越大）
 */
static void kalman_init(KalmanFilter2D *kf, float q, float r) {
    kf->x_estimate = 0;
    kf->y_estimate = 0;
    kf->x_error = 1.0f;
    kf->y_error = 1.0f;
    kf->process_noise = q;
    kf->measurement_noise = r;
    kf->initialized = false;
}

/**
 * 卡尔曼滤波更新
 * @param kf            滤波器实例
 * @param measured_x    测量X值
 * @param measured_y    测量Y值
 * @param out_x         滤波后X输出
 * @param out_y         滤波后Y输出
 */
static void kalman_update(KalmanFilter2D *kf, float measured_x, float measured_y,
                           float *out_x, float *out_y) {
    if (!kf->initialized) {
        /* 第一次测量，直接使用测量值 */
        kf->x_estimate = measured_x;
        kf->y_estimate = measured_y;
        kf->initialized = true;
        *out_x = measured_x;
        *out_y = measured_y;
        return;
    }

    /* 预测步骤：状态不变模型（笔的位置预测 = 上一次估计） */
    float x_pred = kf->x_estimate;
    float y_pred = kf->y_estimate;
    float x_err_pred = kf->x_error + kf->process_noise;
    float y_err_pred = kf->y_error + kf->process_noise;

    /* 更新步骤：计算卡尔曼增益 */
    float kx = x_err_pred / (x_err_pred + kf->measurement_noise);
    float ky = y_err_pred / (y_err_pred + kf->measurement_noise);

    /* 融合预测与测量 */
    kf->x_estimate = x_pred + kx * (measured_x - x_pred);
    kf->y_estimate = y_pred + ky * (measured_y - y_pred);

    /* 更新误差协方差 */
    kf->x_error = (1.0f - kx) * x_err_pred;
    kf->y_error = (1.0f - ky) * y_err_pred;

    *out_x = kf->x_estimate;
    *out_y = kf->y_estimate;
}

/**
 * 重置卡尔曼滤波器（新笔画开始时调用）
 */
static void kalman_reset(KalmanFilter2D *kf) {
    kf->initialized = false;
    kf->x_error = 1.0f;
    kf->y_error = 1.0f;
}

/* ========== 抖动检测 ========== */

/**
 * 检测坐标是否为抖动（笔静止时传感器的微小抖动）
 * 如果两次坐标之间的距离小于阈值，视为抖动并丢弃
 *
 * @param x             当前X坐标
 * @param y             当前Y坐标
 * @param threshold     抖动阈值（坐标单位）
 * @return true表示是抖动，应丢弃
 */
static bool is_jitter(float x, float y, float threshold) {
    float dx = x - s_last_valid_x;
    float dy = y - s_last_valid_y;
    float distance_sq = dx * dx + dy * dy;

    return (distance_sq < threshold * threshold);
}

/* ========== 点阵码图像解码 ========== */

/**
 * 从32x32灰度图像中解码Anoto点阵图案
 *
 * 解码步骤：
 * 1. 二值化：将灰度图转为黑白图
 * 2. 点检测：定位图案中的各个墨点位置
 * 3. 网格对齐：将检测到的点对齐到规则网格
 * 4. 编码读取：根据点相对于网格中心的偏移方向读取编码值
 * 5. 坐标计算：将编码序列映射为全局坐标
 *
 * @param pixels       32x32灰度图像数据
 * @param quality      图像质量评分
 * @param out_x        解码输出X坐标
 * @param out_y        解码输出Y坐标
 * @param out_page_id  解码输出页面ID
 * @return 0成功, -1解码失败
 */
static int decode_dot_pattern(const uint8_t *pixels, uint8_t quality,
                               uint32_t *out_x, uint32_t *out_y,
                               uint32_t *out_page_id) {
    /* 步骤1：自适应二值化 */
    uint8_t threshold = 128;

    /* 根据图像亮度动态调整阈值（Otsu方法简化版） */
    uint32_t histogram[256] = {0};
    uint16_t i;
    for (i = 0; i < SENSOR_PIXELS; i++) {
        histogram[pixels[i]]++;
    }

    /* 寻找双峰之间的谷值作为阈值 */
    uint32_t total = SENSOR_PIXELS;
    uint32_t sum = 0;
    for (i = 0; i < 256; i++) {
        sum += i * histogram[i];
    }

    uint32_t sum_bg = 0;
    uint32_t weight_bg = 0;
    float max_variance = 0;

    for (i = 0; i < 256; i++) {
        weight_bg += histogram[i];
        if (weight_bg == 0) continue;

        uint32_t weight_fg = total - weight_bg;
        if (weight_fg == 0) break;

        sum_bg += i * histogram[i];
        float mean_bg = (float)sum_bg / weight_bg;
        float mean_fg = (float)(sum - sum_bg) / weight_fg;

        float diff = mean_bg - mean_fg;
        float variance = (float)weight_bg * weight_fg * diff * diff;

        if (variance > max_variance) {
            max_variance = variance;
            threshold = (uint8_t)i;
        }
    }

    /* 步骤2：二值化并检测墨点中心 */
    uint8_t dot_count = 0;
    int16_t dot_x[64], dot_y[64];  /* 最多检测64个点 */

    for (int row = 1; row < SENSOR_HEIGHT - 1; row++) {
        for (int col = 1; col < SENSOR_WIDTH - 1; col++) {
            uint8_t center = pixels[row * SENSOR_WIDTH + col];
            if (center < threshold) {
                /* 暗像素，检查是否为局部极小值（简单的点中心检测） */
                uint8_t up    = pixels[(row - 1) * SENSOR_WIDTH + col];
                uint8_t down  = pixels[(row + 1) * SENSOR_WIDTH + col];
                uint8_t left  = pixels[row * SENSOR_WIDTH + (col - 1)];
                uint8_t right = pixels[row * SENSOR_WIDTH + (col + 1)];

                if (center <= up && center <= down &&
                    center <= left && center <= right) {
                    if (dot_count < 64) {
                        dot_x[dot_count] = col;
                        dot_y[dot_count] = row;
                        dot_count++;
                    }
                }
            }
        }
    }

    /* 至少需要检测到4个点才能解码 */
    if (dot_count < 4) {
        return -1;
    }

    /* 步骤3-5：调用点阵码解码器（核心算法在dot_decoder模块中） */
    DotDecodeResult result;
    int ret = dot_decoder_process(dot_x, dot_y, dot_count, &result);

    if (ret == 0) {
        *out_x = result.coordinate_x;
        *out_y = result.coordinate_y;
        *out_page_id = result.page_id;
        return 0;
    }

    return -1;
}

/* ========== 压力值处理 ========== */

/**
 * 处理原始ADC压力值
 * 12位ADC → 归一化并应用非线性映射
 *
 * @param raw_adc  原始ADC值（0-4095）
 * @return 处理后的压力值（0-4095，非线性映射后）
 */
static uint16_t process_pressure(uint16_t raw_adc) {
    /* 去除静态偏移（笔尖自重产生的基础压力） */
    const uint16_t offset = 200;
    if (raw_adc < offset) {
        return 0;
    }
    uint16_t adjusted = raw_adc - offset;

    /* 非线性映射（平方根曲线，使轻触更灵敏） */
    float normalized = (float)adjusted / (4095.0f - offset);
    float mapped = sqrtf(normalized);

    return (uint16_t)(mapped * 4095);
}

/* ========== 坐标计算主任务 ========== */

/**
 * 坐标计算任务（FreeRTOS任务函数）
 *
 * 运行流程：
 * 1. 从图像数据队列接收帧元数据
 * 2. 解码点阵图案获得原始坐标
 * 3. 卡尔曼滤波去噪
 * 4. 抖动检测
 * 5. 坐标打包并放入BLE发送队列
 */
void coordinate_task(void *pvParameters) {
    (void)pvParameters;

    ImageFrameMetadata frame;
    CoordinatePacket packet;

    /* 初始化卡尔曼滤波器 */
    /* Q=0.1 跟踪速度适中, R=0.5 中等滤波强度 */
    kalman_init(&s_kalman, 0.1f, 0.5f);

    /* 抖动检测阈值（坐标单位，约0.1mm） */
    const float jitter_threshold = 3.0f;

    while (1) {
        /* 阻塞等待图像帧数据 */
        if (xQueueReceive(g_image_data_queue, &frame, portMAX_DELAY) != pdTRUE) {
            continue;
        }

        /* 解码点阵图案 */
        uint32_t raw_x, raw_y, page_id;
        int decode_ret = decode_dot_pattern(frame.pixel_buffer, frame.quality_score,
                                             &raw_x, &raw_y, &page_id);

        if (decode_ret != 0) {
            s_decode_failures++;
            continue;
        }
        s_total_decoded++;

        /* 卡尔曼滤波 */
        float filtered_x, filtered_y;
        kalman_update(&s_kalman, (float)raw_x, (float)raw_y,
                      &filtered_x, &filtered_y);

        /* 抖动检测 */
        if (is_jitter(filtered_x, filtered_y, jitter_threshold)) {
            continue;   /* 丢弃抖动数据 */
        }

        /* 更新最后有效坐标 */
        s_last_valid_x = filtered_x;
        s_last_valid_y = filtered_y;

        /* 处理压力值 */
        uint16_t pressure = process_pressure(frame.pressure_raw);

        /* 构建坐标数据包 */
        packet.raw_x = (uint32_t)filtered_x;
        packet.raw_y = (uint32_t)filtered_y;
        packet.pressure = pressure;
        packet.timestamp_ms = frame.timestamp_ms;
        packet.page_id = page_id;
        packet.pen_state = 0;  /* 书写中 */

        /* 放入BLE发送队列（非阻塞，满则丢弃最老的） */
        if (xQueueSend(g_coordinate_queue, &packet, 0) != pdTRUE) {
            /* 队列满，读出一个旧数据再写入 */
            CoordinatePacket dummy;
            xQueueReceive(g_coordinate_queue, &dummy, 0);
            xQueueSend(g_coordinate_queue, &packet, 0);
        }
    }
}