#include "lib_cal_creep_resistance.h"

#define CREEP_VALUE             1
#define CREEP_INTERVAL_FRAME    5

uint8_t g_creep_value = 0;
uint8_t g_creep_interval_frame = 0;


static cal_creep_resistance_t creep_strength_value;

cal_creep_resistance_t * svc_creep_strength_value_get(void)
{
    return &creep_strength_value;
}


void cal_creep_resistance_init(uint8_t creep_strength, uint8_t creep_level)
{
    g_creep_value = creep_strength;
    g_creep_interval_frame = creep_level;
}

void cal_creep_resistance(cal_creep_resistance_t *cfg)
{
    static uint64_t frame_count = 0;
    uint16_t count = cfg->x_max * cfg->y_max;
    int16_t matrix_deviation = 0;
    frame_count ++;

    do{
        count--;

        if (cfg->matrix_real[count] < 2)
        {
            cfg->matrix_display[count] = 0;
            cfg->matrix_creep[count] = 0;
            continue;
        }

        if (!(frame_count % g_creep_interval_frame))
        {
            matrix_deviation = (cfg->matrix_real[count] > cfg->matrix_stab[count]) ? 
                                (cfg->matrix_real[count] - cfg->matrix_stab[count]) : (cfg->matrix_stab[count] - cfg->matrix_real[count]);

            if (matrix_deviation > g_creep_value)
            {

            }
            else 
            {
                matrix_deviation = (cfg->matrix_real[count] > cfg->matrix_stab[count]) ? matrix_deviation : -matrix_deviation;
                cfg->matrix_creep[count] += matrix_deviation;
            }

            cfg->matrix_stab[count] = cfg->matrix_real[count];
        }

        cfg->matrix_display[count] = (cfg->matrix_real[count] > cfg->matrix_creep[count]) ? 
                                    (cfg->matrix_real[count] - cfg->matrix_creep[count]) : cfg->matrix_real[count];

        if (cfg->matrix_real[count] < cfg->matrix_creep[count])
        {
            cfg->matrix_creep[count] = 0;
        }

    } while(count);

}

/* 四宫格并行入口，兼容原 cal_creep_resistance_t 配置 */
void cal_creep_resistance_4tile(const cal_creep_resistance_t *cfg)
{
    /* 原矩阵参数 */
    uint8_t x_max = cfg->x_max;   /* 62 */
    uint8_t y_max = cfg->y_max;   /* 68 */
    uint8_t *real  = cfg->matrix_real;
    uint8_t *stab  = cfg->matrix_stab;
    uint8_t *creep = cfg->matrix_creep;
    uint8_t *disp  = cfg->matrix_display;

    /* 静态变量：四块共享同一帧号（线程安全只读） */
    static uint64_t frame_count = 0;
#pragma omp atomic update
    frame_count++;          /* 原子+1，防止竞态 */

    /* 均分四块 */
    uint8_t w2 = x_max / 2; /* 31 */
    uint8_t h2 = y_max / 2; /* 34 */

#pragma omp parallel for collapse(2) schedule(static) if(1)
    for (int by = 0; by < 2; ++by)
    {
        for (int bx = 0; bx < 2; ++bx)
        {
            /* 本块逻辑区域 */
            uint8_t x0 = bx * w2;
            uint8_t y0 = by * h2;
            uint8_t x1 = (bx == 1) ? x_max : x0 + w2;
            uint8_t y1 = (by == 1) ? y_max : y0 + h2;
            uint8_t bw = x1 - x0;
            uint8_t bh = y1 - y0;

            /* 基指针（按行主序） */
            uint8_t *b_real  = real  + y0 * x_max + x0;
            uint8_t *b_stab  = stab  + y0 * x_max + x0;
            uint8_t *b_creep = creep + y0 * x_max + x0;
            uint8_t *b_disp  = disp  + y0 * x_max + x0;

            /* 对这块逐像素跑原算法 */
            for (uint8_t yy = 0; yy < bh; ++yy)
            {
                for (uint8_t xx = 0; xx < bw; ++xx)
                {
                    uint16_t idx = yy * x_max + xx; /* 全局跨度 */

                    uint8_t r = b_real[idx];
                    if (r < 2)
                    {
                        b_disp[idx]  = 0;
                        b_creep[idx] = 0;
                        continue;
                    }

                    /* UPDATE 阶段：只在间隔帧做 */
                    if ((frame_count % g_creep_interval_frame) == 0)
                    {
                        int16_t s   = b_stab[idx];
                        int16_t dev = (r > s) ? (r - s) : (s - r);
                        if (dev <= g_creep_value)
                        {
                            int16_t c  = b_creep[idx];
                            c += (r > s) ? dev : -dev;
                            b_creep[idx] = (uint8_t)(c < 0 ? 0 : c);
                        }
                        b_stab[idx] = r;
                    }

                    /* DISPLAY 阶段 */
                    uint8_t c = b_creep[idx];
                    b_disp[idx] = (r > c) ? r - c : 0;
                    if (r < c) b_creep[idx] = 0;
                }
            }
        }
    }
}