#include "user_line_process.h"
#include "user_image_process.h"
#include <math.h>
#include <string.h>

/*============================================================
 * 全局变量定义
 *============================================================*/
IFX_ALIGN(4) float center_line[MAX_EDGE_PTS][2];
uint16 center_line_num = 0;
IFX_ALIGN(4) float left_edge[MAX_EDGE_PTS][2];
IFX_ALIGN(4) float right_edge[MAX_EDGE_PTS][2];
uint16 left_edge_num = 0, right_edge_num = 0;
IFX_ALIGN(4) float inflection_point[2] = {0.0f, 0.0f};
uint8 inflection_found = 0;
IFX_ALIGN(4) float inflection_point_r[2] = {0.0f, 0.0f};
uint8 inflection_found_r = 0;

#define MAZE_THRESHOLD  128   /* 二值化阈值: <128 背景(黑), >=128 走线(白) */

/* 拐点检测参数 */
#define INF_LOOKAHEAD  10      /* 前后步长 */
#define INF_ANGLE_COS  0.7f    /* cos(45.6°) */

/* 中线平滑参数 */
#define CENTER_SMOOTH_WIN       7
#define CENTER_OUTLIER_LIMIT    7.0f
#define CENTER_MAX_DX_PER_ROW   3.0f

static float clamp_float(float value, float min_value, float max_value)
{
    if (value < min_value) return min_value;
    if (value > max_value) return max_value;
    return value;
}

static int abs_int(int value)
{
    return value < 0 ? -value : value;
}

static void sort_small_float(float data[], int num)
{
    int i, j;

    for (i = 1; i < num; i++) {
        float key = data[i];
        j = i - 1;
        while (j >= 0 && data[j] > key) {
            data[j + 1] = data[j];
            j--;
        }
        data[j + 1] = key;
    }
}

static void interpolate_edge_rows(const int16 edge_x[], float interp_x[], uint8 valid[], int h)
{
    int y, prev_y, next_y, fill_y;

    for (y = 0; y < h; y++) {
        if (edge_x[y] >= 0 && edge_x[y] < 32767) {
            interp_x[y] = (float)edge_x[y];
            valid[y] = 1;
        } else {
            interp_x[y] = 0.0f;
            valid[y] = 0;
        }
    }

    prev_y = -1;
    for (y = 0; y < h; y++) {
        if (!valid[y]) continue;

        if (prev_y >= 0 && y - prev_y > 1) {
            next_y = y;
            for (fill_y = prev_y + 1; fill_y < next_y; fill_y++) {
                float ratio = (float)(fill_y - prev_y) / (float)(next_y - prev_y);
                interp_x[fill_y] = interp_x[prev_y] + (interp_x[next_y] - interp_x[prev_y]) * ratio;
                valid[fill_y] = 1;
            }
        }
        prev_y = y;
    }
}

static uint8 solve_3x3(float mat[3][4], float result[3])
{
    int col, row, pivot_row, i;

    for (col = 0; col < 3; col++) {
        float pivot_abs = fabsf(mat[col][col]);
        pivot_row = col;

        for (row = col + 1; row < 3; row++) {
            float value_abs = fabsf(mat[row][col]);
            if (value_abs > pivot_abs) {
                pivot_abs = value_abs;
                pivot_row = row;
            }
        }

        if (pivot_abs < 1e-6f) return 0;

        if (pivot_row != col) {
            for (i = col; i < 4; i++) {
                float temp = mat[col][i];
                mat[col][i] = mat[pivot_row][i];
                mat[pivot_row][i] = temp;
            }
        }

        for (row = 0; row < 3; row++) {
            float factor;
            if (row == col) continue;

            factor = mat[row][col] / mat[col][col];
            for (i = col; i < 4; i++) {
                mat[row][i] -= factor * mat[col][i];
            }
        }
    }

    for (row = 0; row < 3; row++) {
        result[row] = mat[row][3] / mat[row][row];
    }
    return 1;
}

static float eval_center_quad(const float coef[3], int y, int h)
{
    float yn = ((float)y - (float)(h - 1)) / (float)h;
    return coef[0] + coef[1] * yn + coef[2] * yn * yn;
}

static uint8 fit_center_quad(const float x[], const uint8 valid[], int h,
                             const float *ref_coef, float residual_limit,
                             float coef[3])
{
    float mat[3][4] = {{0}};
    int y, count = 0;

    for (y = 0; y < h; y++) {
        float yn, yn2, w, ref_x;

        if (!valid[y]) continue;

        if (ref_coef != 0) {
            ref_x = eval_center_quad(ref_coef, y, h);
            if (fabsf(x[y] - ref_x) > residual_limit) continue;
        }

        yn = ((float)y - (float)(h - 1)) / (float)h;
        yn2 = yn * yn;
        w = 1.0f + 2.0f * (float)y / (float)(h - 1);

        mat[0][0] += w;
        mat[0][1] += w * yn;
        mat[0][2] += w * yn2;
        mat[0][3] += w * x[y];

        mat[1][0] += w * yn;
        mat[1][1] += w * yn2;
        mat[1][2] += w * yn2 * yn;
        mat[1][3] += w * yn * x[y];

        mat[2][0] += w * yn2;
        mat[2][1] += w * yn2 * yn;
        mat[2][2] += w * yn2 * yn2;
        mat[2][3] += w * yn2 * x[y];

        count++;
    }

    if (count < 8) return 0;
    return solve_3x3(mat, coef);
}

static void smooth_center_rows(const float raw_x[], const uint8 valid[], float smooth_x[], int h)
{
    static float median_x[MAX_EDGE_PTS];
    static float avg_x[MAX_EDGE_PTS];
    float coef_first[3], coef_second[3];
    uint8 fit_ok;
    int y, k, half, has_base;
    float base_x;

    half = CENTER_SMOOTH_WIN / 2;

    for (y = 0; y < h; y++) {
        float window[CENTER_SMOOTH_WIN];
        int num = 0;

        median_x[y] = raw_x[y];
        avg_x[y] = raw_x[y];
        smooth_x[y] = raw_x[y];

        if (!valid[y]) continue;

        for (k = y - half; k <= y + half; k++) {
            if (0 <= k && k < h && valid[k]) {
                window[num++] = raw_x[k];
            }
        }

        if (num > 0) {
            sort_small_float(window, num);
            median_x[y] = window[num / 2];
        }
    }

    for (y = 0; y < h; y++) {
        float sum = 0.0f;
        float weight_sum = 0.0f;

        if (!valid[y]) continue;

        for (k = y - half; k <= y + half; k++) {
            if (0 <= k && k < h && valid[k]) {
                float weight = (float)(half + 1 - abs_int(k - y));
                sum += median_x[k] * weight;
                weight_sum += weight;
            }
        }

        if (weight_sum > 1e-6f) {
            avg_x[y] = sum / weight_sum;
        }
    }

    fit_ok = fit_center_quad(avg_x, valid, h, 0, 0.0f, coef_first);
    if (fit_ok) {
        fit_ok = fit_center_quad(avg_x, valid, h, coef_first, CENTER_OUTLIER_LIMIT, coef_second);
    }

    for (y = 0; y < h; y++) {
        if (!valid[y]) continue;

        if (fit_ok) {
            float fit_x = eval_center_quad(coef_second, y, h);
            smooth_x[y] = avg_x[y] * 0.45f + fit_x * 0.55f;
        } else {
            smooth_x[y] = avg_x[y];
        }
        smooth_x[y] = clamp_float(smooth_x[y], 0.0f, (float)(MT9V03X_1_W - 1));
    }

    has_base = 0;
    base_x = 0.0f;
    for (y = h - 1; y >= 0; y--) {
        if (!valid[y]) continue;

        if (has_base) {
            float dx = smooth_x[y] - base_x;
            smooth_x[y] = base_x + clamp_float(dx, -CENTER_MAX_DX_PER_ROW, CENTER_MAX_DX_PER_ROW);
        }

        base_x = smooth_x[y];
        has_base = 1;
    }
}

/*============================================================
 * 4方向表 (迷宫法贴墙走)
 *   dir: 0=上 1=右 2=下 3=左
 *============================================================*/
static const int8 dir_front[4][2]       = {{ 0,-1}, { 1, 0}, { 0, 1}, {-1, 0}};
static const int8 dir_frontleft[4][2]   = {{-1,-1}, { 1,-1}, { 1, 1}, {-1, 1}};
static const int8 dir_frontright[4][2]  = {{ 1,-1}, { 1, 1}, {-1, 1}, {-1,-1}};

/*============================================================
 * 检查指定列在拐点行上下3行范围内是否存在黑白跳变
 *============================================================*/
static uint8 check_col_transition(uint8 *binary, int iw, int ih, int col, int row)
{
    int r, r_start = row - 3;
    int r_end   = row + 3;

    if (r_start < 0)  r_start = 0;
    if (r_end  >= ih) r_end   = ih - 1;
    if (r_start >= r_end) return 0;

    for (r = r_start + 1; r <= r_end; r++) {
        if (binary[(r - 1) * iw + col] != binary[r * iw + col]) {
            return 1;
        }
    }
    return 0;
}

/*============================================================
 * 左手定则迷宫寻线 (4方向贴墙走)
 *
 * 沿白色赛道左边缘追踪: 始终让"左前方"贴着黑色背景.
 *
 * 左手规则:
 *   - 前方为黑 → 右转 (dir+1), turn++
 *   - 前方为白、左前为黑 → 直走 (贴左墙)
 *   - 前方为白、左前为白 → 左前走 (赛道左弯), 同时左转 (dir+3)
 *   - turn >= 4 视为死路退出
 *============================================================*/
static uint16 findline_lefthand(uint8 *binary, int iw, int ih,
                                 int start_x, int start_y,
                                 int16 pts[][2], uint16 max_pts) {
    int x = start_x, y = start_y;
    int dir = 0;
    int turn = 0;
    uint16 step = 0;

    while (step < max_pts && 1 <= x && x < iw - 1 && 1 <= y && y < ih - 1 && turn < 4) {

        int fx  = x + dir_front[dir][0];
        int fy  = y + dir_front[dir][1];
        int flx = x + dir_frontleft[dir][0];
        int fly = y + dir_frontleft[dir][1];

        uint8 front      = binary[fy * iw + fx];
        uint8 frontleft  = binary[fly * iw + flx];

        if (front < MAZE_THRESHOLD) {
            dir = (dir + 1) & 3;
            turn++;
        } else if (frontleft < MAZE_THRESHOLD) {
            x = fx;
            y = fy;
            pts[step][0] = (int16)x;
            pts[step][1] = (int16)y;
            step++;
            turn = 0;
        } else {
            x = flx;
            y = fly;
            dir = (dir + 3) & 3;
            pts[step][0] = (int16)x;
            pts[step][1] = (int16)y;
            step++;
            turn = 0;
        }
    }
    /* 拐点检测: 角度突变 → 记录拐点, 继续追踪 */
    if (!inflection_found) {
        int n = (int)step, s = INF_LOOKAHEAD;
        if (n >= 2 * s) {
            int i;
            for (i = s + 10; i + s < n; i++) {
                if (pts[i][1] >= MT9V03X_1_H - 8) continue;
                float dx_in  = (float)(pts[i][0] - pts[i - s][0]);
                float dy_in  = (float)(pts[i][1] - pts[i - s][1]);
                float dx_out = (float)(pts[i + s][0] - pts[i][0]);
                float dy_out = (float)(pts[i + s][1] - pts[i][1]);
                float len1 = sqrtf(dx_in * dx_in + dy_in * dy_in);
                float len2 = sqrtf(dx_out * dx_out + dy_out * dy_out);
                if (len1 < 1e-6f || len2 < 1e-6f) continue;
                if ((dx_in * dx_out + dy_in * dy_out) / (len1 * len2) < INF_ANGLE_COS) {
                    int candidate_row = pts[i][1];
                    if (candidate_row <= 30) {
                        int j, has_another = 0;
                        for (j = i + 1; j + s < n; j++) {
                            float dx_in2  = (float)(pts[j][0] - pts[j - s][0]);
                            float dy_in2  = (float)(pts[j][1] - pts[j - s][1]);
                            float dx_out2 = (float)(pts[j + s][0] - pts[j][0]);
                            float dy_out2 = (float)(pts[j + s][1] - pts[j][1]);
                            float len1b = sqrtf(dx_in2 * dx_in2 + dy_in2 * dy_in2);
                            float len2b = sqrtf(dx_out2 * dx_out2 + dy_out2 * dy_out2);
                            if (len1b < 1e-6f || len2b < 1e-6f) continue;
                            if ((dx_in2 * dx_out2 + dy_in2 * dy_out2) / (len1b * len2b) < INF_ANGLE_COS) {
                                has_another = 1;
                                break;
                            }
                        }
                        if (has_another) continue;
                    }
                    if (check_col_transition(binary, iw, ih, 2, pts[i][1])) {
                        inflection_point[0] = (float)pts[i][0];
                        inflection_point[1] = (float)pts[i][1];
                        inflection_found  = 1;
                        break;
                    }
                }
            }
        }
    }
    return step;
}

/*============================================================
 * 右手定则迷宫寻线 (4方向贴墙走)
 *
 * 沿白色赛道右边缘追踪: 始终让"右前方"贴着黑色背景.
 *
 * 右手规则:
 *   - 前方为黑 → 左转 (dir+3), turn++
 *   - 前方为白、右前为黑 → 直走 (贴右墙)
 *   - 前方为白、右前为白 → 右前走 (赛道右弯), 同时右转 (dir+1)
 *   - turn >= 4 视为死路退出
 *============================================================*/
static uint16 findline_righthand(uint8 *binary, int iw, int ih,
                                  int start_x, int start_y,
                                  int16 pts[][2], uint16 max_pts) {
    int x = start_x, y = start_y;
    int dir = 0;
    int turn = 0;
    uint16 step = 0;

    while (step < max_pts && 1 <= x && x < iw - 1 && 1 <= y && y < ih - 1 && turn < 4) {

        int fx  = x + dir_front[dir][0];
        int fy  = y + dir_front[dir][1];
        int frx = x + dir_frontright[dir][0];
        int fry = y + dir_frontright[dir][1];

        uint8 front       = binary[fy * iw + fx];
        uint8 frontright  = binary[fry * iw + frx];

        if (front < MAZE_THRESHOLD) {
            dir = (dir + 3) & 3;
            turn++;
        } else if (frontright < MAZE_THRESHOLD) {
            x = fx;
            y = fy;
            pts[step][0] = (int16)x;
            pts[step][1] = (int16)y;
            step++;
            turn = 0;
        } else {
            x = frx;
            y = fry;
            dir = (dir + 1) & 3;
            pts[step][0] = (int16)x;
            pts[step][1] = (int16)y;
            step++;
            turn = 0;
        }
    }
    /* 拐点检测: 角度突变 → 记录拐点, 继续追踪 */
    if (!inflection_found_r) {
        int n = (int)step, s = INF_LOOKAHEAD;
        if (n >= 2 * s) {
            int i;
            for (i = s + 10; i + s < n; i++) {
                if (pts[i][1] >= MT9V03X_1_H - 8) continue;
                float dx_in  = (float)(pts[i][0] - pts[i - s][0]);
                float dy_in  = (float)(pts[i][1] - pts[i - s][1]);
                float dx_out = (float)(pts[i + s][0] - pts[i][0]);
                float dy_out = (float)(pts[i + s][1] - pts[i][1]);
                float len1 = sqrtf(dx_in * dx_in + dy_in * dy_in);
                float len2 = sqrtf(dx_out * dx_out + dy_out * dy_out);
                if (len1 < 1e-6f || len2 < 1e-6f) continue;
                if ((dx_in * dx_out + dy_in * dy_out) / (len1 * len2) < INF_ANGLE_COS) {
                    int candidate_row = pts[i][1];
                    if (candidate_row <= 30) {
                        int j, has_another = 0;
                        for (j = i + 1; j + s < n; j++) {
                            float dx_in2  = (float)(pts[j][0] - pts[j - s][0]);
                            float dy_in2  = (float)(pts[j][1] - pts[j - s][1]);
                            float dx_out2 = (float)(pts[j + s][0] - pts[j][0]);
                            float dy_out2 = (float)(pts[j + s][1] - pts[j][1]);
                            float len1b = sqrtf(dx_in2 * dx_in2 + dy_in2 * dy_in2);
                            float len2b = sqrtf(dx_out2 * dx_out2 + dy_out2 * dy_out2);
                            if (len1b < 1e-6f || len2b < 1e-6f) continue;
                            if ((dx_in2 * dx_out2 + dy_in2 * dy_out2) / (len1b * len2b) < INF_ANGLE_COS) {
                                has_another = 1;
                                break;
                            }
                        }
                        if (has_another) continue;
                    }
                    if (check_col_transition(binary, iw, ih, 138, pts[i][1])) {
                        inflection_point_r[0] = (float)pts[i][0];
                        inflection_point_r[1] = (float)pts[i][1];
                        inflection_found_r  = 1;
                        break;
                    }
                }
            }
        }
    }
    return step;
}

/*============================================================
 * 赛道边界检测 (迷宫法)
 *
 * 从图像底部中心找到白色走线上的一点作为起点,
 * 分别用左手定则和右手定则向上跟踪走线的左右边缘.
 *
 * 新增: 当中心点未命中赛道时, 全宽扫描底行找最近赛道段,
 *       解决车体偏航导致整帧丢弃的问题.
 *============================================================*/
void find_circuit_edges(uint8 *binary, uint16 w, uint16 h,
                         int16 start_x, int16 start_y,
                         int16 left_pts[][2], uint16 *left_num,
                         int16 right_pts[][2], uint16 *right_num) {
    int iw = (int)w, ih = (int)h;
    int cy = (int)start_y;
    int left_start, right_start;
    int row_base, x;

    if (cy > ih - 2) cy = ih - 2;
    if (cy < 1) cy = 1;

    /* ---- 1. 底行从左右两侧向内扫描找边界 ---- */
    row_base   = cy * iw;
    left_start = -1;
    right_start = -1;

    for (x = 1; x < iw - 1; x++) {
        if (binary[row_base + x] >= MAZE_THRESHOLD &&
            binary[row_base + x - 1] < MAZE_THRESHOLD) {
            left_start = x;
            break;
        }
    }

    for (x = iw - 2; x >= 1; x--) {
        if (binary[row_base + x] >= MAZE_THRESHOLD &&
            binary[row_base + x + 1] < MAZE_THRESHOLD) {
            right_start = x;
            break;
        }
    }

    if (left_start < 0 || right_start < 0) {
        *left_num  = 0;
        *right_num = 0;
        return;
    }

    /* ---- 2. 左右手定则分别从各自边缘向上追踪 ---- */
    *left_num  = findline_lefthand(binary, iw, ih,
                                    left_start, cy,
                                    left_pts, *left_num);
    *right_num = findline_righthand(binary, iw, ih,
                                     right_start, cy,
                                     right_pts, *right_num);
}

/*============================================================
 * 中线计算
 *
 * 同行左取最大 x、右取最小 x, 先插值补齐缺失行,
 * 再做中值/三角平滑和鲁棒二次拟合.
 *============================================================*/

void compute_center_line_interp(int16 left[][2], uint16 left_num,
                                 int16 right[][2], uint16 right_num,
                                 float center[][2], uint16 *center_num)
{
    static int16 left_max_x[MAX_EDGE_PTS];
    static int16 right_min_x[MAX_EDGE_PTS];
    static float left_interp_x[MAX_EDGE_PTS];
    static float right_interp_x[MAX_EDGE_PTS];
    static float cx_raw[MAX_EDGE_PTS];
    static float cx_smooth[MAX_EDGE_PTS];
    static uint8 left_valid[MAX_EDGE_PTS];
    static uint8 right_valid[MAX_EDGE_PTS];
    static uint8 center_valid[MAX_EDGE_PTS];
    uint16 i;
    int y;

    if (left_num < 2 || right_num < 2) {
        *center_num = 0;
        return;
    }

    /* ---- 1. 初始化 ---- */
    for (i = 0; i < MAX_EDGE_PTS; i++) {
        left_max_x[i]  = -1;
        right_min_x[i] = 32767;
        cx_raw[i] = -1.0f;
        cx_smooth[i] = -1.0f;
        left_valid[i] = 0;
        right_valid[i] = 0;
        center_valid[i] = 0;
    }

    /* ---- 2. 左取最大, 右取最小 (取内侧边界点) ---- */
    for (i = 0; i < left_num; i++) {
        y = (int)left[i][1];
        if (0 <= y && y < MAX_EDGE_PTS) {
            if (left[i][0] > left_max_x[y])
                left_max_x[y] = left[i][0];
        }
    }
    for (i = 0; i < right_num; i++) {
        y = (int)right[i][1];
        if (0 <= y && y < MAX_EDGE_PTS) {
            if (right[i][0] < right_min_x[y])
                right_min_x[y] = right[i][0];
        }
    }

    /* ---- 3. 边线缺失行插值, 避免同一行没有配对点造成中线稀疏 ---- */
    interpolate_edge_rows(left_max_x, left_interp_x, left_valid, (int)MT9V03X_1_H);
    interpolate_edge_rows(right_min_x, right_interp_x, right_valid, (int)MT9V03X_1_H);

    /* ---- 4. 每行原始中线 ---- */
    for (y = 0; y < (int)MT9V03X_1_H; y++) {
        if (left_valid[y] && right_valid[y]) {
            cx_raw[y] = (left_interp_x[y] + right_interp_x[y]) * 0.5f;
            center_valid[y] = 1;
        }
    }

    smooth_center_rows(cx_raw, center_valid, cx_smooth, (int)MT9V03X_1_H);

    /* ---- 5. 输出所有有效行的平滑中线 ---- */
    *center_num = 0;
    for (y = 0; y < (int)MT9V03X_1_H && *center_num < MAX_EDGE_PTS; y++) {
        if (center_valid[y]) {
            center[*center_num][0] = cx_smooth[y];
            center[*center_num][1] = (float)y;
            (*center_num)++;
        }
    }

    if (*center_num < 2) {
        *center_num = 0;
        return;
    }
}

/*============================================================
 * 一帧图像处理主入口
 *
 * 流水线:
 *   binary → find_circuit_edges (迷宫法)
 *         → compute_center_line_interp (插值、平滑、拟合中线)
 *         → 左右边线直接输出
 *         → detect_inflection_point (拐点检测)
 *
 * 输出全局变量: center_line, left_edge, right_edge, inflection_point
 *============================================================*/
void line_process_run(uint8 *binary)
{
    static int16 left_pts[MAX_EDGE_PTS][2];
    static int16 right_pts[MAX_EDGE_PTS][2];

    uint16 left_num = MAX_EDGE_PTS;
    uint16 right_num = MAX_EDGE_PTS;
    uint16 i;

    /* 清除上帧拐点标志 (trace 函数只置位, 不清零) */
    inflection_found   = 0;
    inflection_found_r = 0;

    /* ---- 1. 迷宫法赛道边缘检测 ---- */
    find_circuit_edges(
        binary,
        MT9V03X_1_W, MT9V03X_1_H,
        (int16)(MT9V03X_1_W / 2),
        (int16)(MT9V03X_1_H - 1),
        left_pts, &left_num,
        right_pts, &right_num);

    if (left_num < 2 || right_num < 2) {
        center_line_num = 0;
        left_edge_num = 0;
        right_edge_num = 0;
        return;
    }

    /* ---- 2. 中线计算 (插值、平滑、拟合) ---- */
    compute_center_line_interp(left_pts, left_num, right_pts, right_num,
                                center_line, &center_line_num);

    /* ---- 3. 左边线输出 ---- */
    left_edge_num = left_num < MAX_EDGE_PTS ? left_num : MAX_EDGE_PTS;
    for (i = 0; i < left_edge_num; i++) {
        left_edge[i][0] = (float)left_pts[i][0];
        left_edge[i][1] = (float)left_pts[i][1];
    }

    /* ---- 4. 右边线输出 ---- */
    right_edge_num = right_num < MAX_EDGE_PTS ? right_num : MAX_EDGE_PTS;
    for (i = 0; i < right_edge_num; i++) {
        right_edge[i][0] = (float)right_pts[i][0];
        right_edge[i][1] = (float)right_pts[i][1];
    }
}

/*============================================================
 * 中线加权偏差 (越靠图像底部权重越大)
 *
 * 仅计入 y >= min_row 的中线点, 上半部分忽略.
 * weight = y + 1  (y=顶部0, y=底部≈79)
 * 底部(近车)权重大, 顶部(远处)权重小
 *
 * 返回: 加权平均中线偏差 (像素), 正=偏右, 负=偏左
 *============================================================*/
float line_compute_weighted_deviation(uint16 image_center_x, uint16 min_row)
{
    float w_sum = 0.0f, dev_sum = 0.0f;
    uint16 i;

    if (center_line_num < 2)
        return 0.0f;

    for (i = 0; i < center_line_num; i++)
    {
        float y = center_line[i][1];
        if (y < (float)min_row) continue;

        float weight = y + 1.0f;
        float dev    = center_line[i][0] - (float)image_center_x;

        dev_sum += dev * weight;
        w_sum   += weight;
    }

    return (w_sum > 0.0f) ? (dev_sum / w_sum) : 0.0f;
}