#include "sensor.h"
#include "ads1256.h"
#include "calibration.h"
#include "comm_protocol.h"

#include <string.h>
#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/sys/atomic.h>

LOG_MODULE_REGISTER(sensor, LOG_LEVEL_INF);

/*
 * 每通道每帧采 17 次取中值。3750 SPS 下：
 *    17 次各 0.27ms = 4.86ms/通道
 *   4 通道 = 19.4ms，20ms 帧周期内刚好用满
 * 中值滤波对脉冲/尖峰干扰的抑制力强于均值
 */
#define AVG_COUNT          17
#define DEADZONE_THRESHOLD 1000

/* MUX 通道配置：4 路差分 */
static const uint8_t mux_channels[4] = { 0x01, 0x23, 0x45, 0x67 };

/* 传感器坐标：S0=FR, S1=BR, S2=BL, S3=FL */
static const float sensor_x[4] = {
    +BOARD_HALF_WIDTH_CM, /* S0: FR */
    +BOARD_HALF_WIDTH_CM, /* S1: BR */
    -BOARD_HALF_WIDTH_CM, /* S2: BL */
    -BOARD_HALF_WIDTH_CM, /* S3: FL */
};
static const float sensor_y[4] = {
    +BOARD_HALF_LENGTH_CM, /* S0: FR */
    -BOARD_HALF_LENGTH_CM, /* S1: BR */
    -BOARD_HALF_LENGTH_CM, /* S2: BL */
    +BOARD_HALF_LENGTH_CM, /* S3: FL */
};

/* --- 内部状态 --- */
static int32_t sensor_offsets[4]; // 去皮的零点偏移值
static int32_t filtered[4];
static atomic_t tare_requested;

/* --- 线程 --- */
#define SENSOR_STACK_SIZE 2048
#define SENSOR_PRIORITY   5
static K_THREAD_STACK_DEFINE(sensor_stack, SENSOR_STACK_SIZE);
static struct k_thread sensor_thread;

/**
 * @brief 对整型数组做原地升序排序。
 *
 * @param arr 待排序的数据缓冲区；这里要求调用方传入可写数组，
 *            因为去皮流程需要直接在采样缓冲区上重排以减少额外拷贝。
 * @param n 数组元素个数；显式传入长度是为了让该内部工具函数只依赖调用现场，
 *          避免隐式假设固定采样数后影响后续维护。
 *
 * @return 无返回值。
 */
static void sort_array(int32_t *arr, int n) {
    for (int i = 0; i < n - 1; i++) {
        for (int j = 0; j < n - i - 1; j++) {
            if (arr[j] > arr[j + 1]) {
                int32_t temp = arr[j];
                arr[j]       = arr[j + 1];
                arr[j + 1]   = temp;
            }
        }
    }
}

/**
 * @brief 执行一次四路传感器去皮，更新每路零点偏移。
 *
 * @param 无。
 *
 * @return 无返回值。
 */
static void do_tare(void) {
    LOG_INF("Taring...");
    int32_t sorted_buf[AVG_COUNT];

    for (int i = 0; i < 4; i++) {
        ads1256_write_reg(ADS1256_REG_MUX, mux_channels[i]);
        ads1256_sync_wakeup();
        for (int k = 0; k < AVG_COUNT; k++) {
            ads1256_wait_drdy(50);
            sorted_buf[k] = ads1256_read_data();
        }
        sort_array(sorted_buf, AVG_COUNT);
        sensor_offsets[i] = sorted_buf[AVG_COUNT / 2];
    }

    LOG_INF(
        "Tare offsets=[%ld,%ld,%ld,%ld]", (long)sensor_offsets[0], (long)sensor_offsets[1], (long)sensor_offsets[2],
        (long)sensor_offsets[3]);
    memset(filtered, 0, sizeof(filtered));
}

/**
 * @brief 完成一帧四通道采集，并写入去皮后的滤波结果。
 *
 * @param 无。
 *
 * @return 无返回值。
 */
static void acquire_cycle(void) {
    int32_t samples[AVG_COUNT];

    for (int ch = 0; ch < 4; ch++) {
        ads1256_write_reg(ADS1256_REG_MUX, mux_channels[ch]);
        ads1256_sync_wakeup();

        for (int k = 0; k < AVG_COUNT; k++) {
            ads1256_wait_drdy(50);
            samples[k] = ads1256_read_data();
        }

        sort_array(samples, AVG_COUNT);
        int32_t median = samples[AVG_COUNT / 2] - sensor_offsets[ch];
        if (median > -DEADZONE_THRESHOLD && median < DEADZONE_THRESHOLD) median = 0;
        filtered[ch] = median;
    }
}

/**
 * @brief 根据四路受力结果计算压力中心（CoP）和总力。
 *
 * @param[out] out_x 输出 CoP 的 X 坐标 (cm)；
 * @param[out] out_y 输出 CoP 的 Y 坐标 (cm)；
 * @param[out] out_force 输出总力值 (kg)，int16 截断；
 * @param[out] out_total 输出 float 精度总力，供 IIR alpha 判断用。
 *
 * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。
 * @retval false 总力不足，CoP 被强制归零以避免在几乎无载荷时放大数值噪声。
 */
static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force, float *out_total) {
    static bool force_valid_state = false;

    const struct cal_runtime *cal = cal_get_working();
    float forces[4];
    float total = 0.0f;

    for (int i = 0; i < 4; i++) {
        float gain = cal->l1_gain_valid ? cal->gain[i] : ADC_TO_FORCE_SCALE;
        forces[i]  = (float)filtered[i] * gain;
        /* 压力传感器不可能产生负力，负值是零漂 */
        if (forces[i] < 0.0f) forces[i] = 0.0f;
        total += forces[i];
    }

    *out_force = (int16_t)total;
    *out_total = total;

    /* 迟滞判定：避免阈值附近反复切换 */
    if (!force_valid_state) {
        if (total < COP_FORCE_ENTER_THRESHOLD) {
            *out_x = 0;
            *out_y = 0;
            return false;
        }
        force_valid_state = true;
    } else {
        if (total < COP_FORCE_EXIT_THRESHOLD) {
            force_valid_state = false;
            *out_x            = 0;
            *out_y            = 0;
            return false;
        }
    }

    float wx = 0.0f, wy = 0.0f;
    for (int i = 0; i < 4; i++) {
        wx += forces[i] * sensor_x[i];
        wy += forces[i] * sensor_y[i];
    }
    *out_x = (int16_t)(wx / total);
    *out_y = (int16_t)(wy / total);
    return true;
}

/**
 * @brief 传感器后台线程主循环，负责采集、去皮处理、CoP 计算和蓝牙发送。
 *
 * @param p1 Zephyr 线程入口预留参数，当前未使用；
 * @param p2 Zephyr 线程入口预留参数，当前未使用；
 * @param p3 Zephyr 线程入口预留参数，当前未使用；
 *
 * @return 无返回值。
 */
static void sensor_thread_fn(void *p1, void *p2, void *p3) {
    (void)p1;
    (void)p2;
    (void)p3;

    uint8_t debug_log_divider = 0;
    int64_t frame_ts          = k_uptime_get();

    while (1) {
        int64_t now      = k_uptime_get();
        int64_t frame_ms = now - frame_ts;
        frame_ts         = now;

        cal_check_update();
        if (cal_execute_pending()) {
            continue;
        }

        /* 去皮请求 */
        if (atomic_cas(&tare_requested, 1, 0)) {
            do_tare();
        }

        /* 采集 */
        int64_t t0 = k_uptime_get();
        acquire_cycle();
        int64_t t1 = k_uptime_get();

        /* CoP 计算 + IIR 平滑 + 发送 */
        static float cop_x_lpf = 0.0f;
        static float cop_y_lpf = 0.0f;

        int16_t cop_x, cop_y, force;
        float total_f;
        bool valid    = compute_cop(&cop_x, &cop_y, &force, &total_f);
        uint8_t flags = valid ? COP_FLAG_FORCE_VALID : 0;
        if (valid) {
            float fx = (float)cop_x;
            float fy = (float)cop_y;

            cal_apply_l2_correction(&fx, &fy);

            /* 重载直通、轻载平滑 */
            float alpha = (total_f >= COP_HEAVY_FORCE) ? 1.0f : COP_LPF_ALPHA_LIGHT;
            cop_x_lpf   = alpha * fx + (1.0f - alpha) * cop_x_lpf;
            cop_y_lpf   = alpha * fy + (1.0f - alpha) * cop_y_lpf;

            cop_x = (int16_t)cop_x_lpf;
            cop_y = (int16_t)cop_y_lpf;
        } else {
            cop_x_lpf = 0.0f;
            cop_y_lpf = 0.0f;
        }
        comm_protocol_send_cop(flags, cop_x, cop_y, force);

        /* 调试日志（5 Hz） */
        if (++debug_log_divider >= 10) {
            debug_log_divider = 0;
            LOG_INF(
                "FR=%.2f\t BR=%.2f\t BL=%.2f\t FL=%.2f\t | total=%d kg | cop=(%d,%d) cm | %s", (double)filtered[0],
                (double)filtered[1], (double)filtered[2], (double)filtered[3], force, cop_x, cop_y,
                valid ? "VALID" : "low");
            LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms);
        }
    }
}

/**
 * @brief 初始化压力传感器模块：ADS1256 硬件 + 初始去皮 + 创建采集线程。
 *
 * 调用后采集线程自动启动，以 50 Hz 循环采集、计算 CoP 并发送 BLE 数据。
 *
 * @retval 0 初始化成功，传感器线程已经启动并完成一次初始去皮。
 * @retval 负值 ADS1256 初始化阶段返回的具体错误码。
 */
int sensor_init(void) {
    int err = ads1256_init();
    if (err) return err;

    /* SELFCAL 后 ADC 模拟链路需几个转换周期才能完全建立，空读排空 pipeline */
    for (int i = 0; i < 4; i++) {
        ads1256_wait_drdy(50);
        (void)ads1256_read_data();
    }

    memset(sensor_offsets, 0, sizeof(sensor_offsets));
    memset(filtered, 0, sizeof(filtered));

    do_tare();

    k_thread_create(
        &sensor_thread, sensor_stack, SENSOR_STACK_SIZE, sensor_thread_fn, NULL, NULL, NULL, SENSOR_PRIORITY, 0,
        K_NO_WAIT);
    k_thread_name_set(&sensor_thread, "sensor");

    return 0;
}

/**
 * @brief 执行四路去皮（零点校准），可从任意线程调用。
 *
 * @return 无返回值。
 */
void sensor_perform_tare(void) {
    atomic_set(&tare_requested, 1);
}