#include "zf_common_headfile.h"
#include "gl_headfile.h"

float (*mid_track)[2];
int32_t mid_track_count;
float pure_angle;
float dx_near;
float (*rpts)[2];
int rpts_num;

void tracking()
{

    if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) {
        track_type = TRACK_RIGHT;
    } else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) {
        track_type = TRACK_LEFT;
    } else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) {
        track_type = TRACK_RIGHT;
    } else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) {
        track_type = TRACK_LEFT;
    }
}

void ElementJudge()
{
    CheckGarage();
    if (garage_type == GARAGE_NONE) {
        CheckCross();
        if (cross_type == CROSS_NONE) {
            CheckCircle();
        }
    }

    if (garage_type != GARAGE_NONE) {
        cross_type  = CROSS_NONE;
        circle_type = CIRCLE_NONE;
    }
}

void ElementRun()
{
    if (garage_type != GARAGE_NONE) {
        RunGarage();
    }

    else if (cross_type != CROSS_NONE) {
        RunCross();
    }

    else if (circle_type != CIRCLE_NONE) {
        RunCircle();
    }
}

void MidLineTrack()
{
    if (cross_type == CROSS_IN) {
        if (track_type == TRACK_LEFT) {
            mid_track = mid_left; // 这是为了预先分配内存
            GetMidLine_Left(pts_far_resample_left + far_Lpt0_rpts0s_id, pts_far_resample_left_count - far_Lpt0_rpts0s_id, mid_left, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
            mid_track_count = pts_far_resample_left_count - far_Lpt0_rpts0s_id;
        } else {
            mid_track = mid_right; // 这是为了预先分配内存
            GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
            mid_track_count = pts_far_resample_right_count - far_Lpt1_rpts1s_id;
        }
    } else {
        if (track_type == TRACK_LEFT) {
            mid_track       = mid_left;
            mid_track_count = mid_left_count;
        } else {
            mid_track       = mid_right;
            mid_track_count = mid_right_count;
        }
    }

    // 车轮对应点 (纯跟踪起始点)
    float cx = InverseMapW[(int)(IMAGE_H * 0.75f)][70];
    float cy = InverseMapH[(int)(IMAGE_H * 0.75f)][70];

    // 找最近点 (起始点中线归一化)
    float min_dist = 1e10;

    int begin_id = -1;
    for (int i = 0; i < mid_track_count; i++) {
        float dx   = mid_track[i][1] - cx;
        float dy   = mid_track[i][0] - cy;
        float dist = Q_sqrt(dx * dx + dy * dy);
        if (dist < min_dist) {
            min_dist = dist;
            begin_id = i;
        }
    }

    if (begin_id >= 0 && mid_track_count - begin_id >= 3) {
        // 归一化中线
        mid_track[begin_id][0] = cy;
        mid_track[begin_id][1] = cx;
        rptsn_num              = sizeof(rptsn) / sizeof(rptsn[0]);
        GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER);

        // 远预锚点位置-
        int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);

        // 近锚点位置
        int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1);

        // 计算远锚点偏差值
        float dx = rptsn[aim_idx][1] - cx;
        float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;
        float dn = (float)Q_sqrt(dx * dx + dy * dy);
        //  float error = -atan2f(dx, dy) * 180 / PI32;

        // 计算近锚点偏差值
        dx_near = rptsn[aim_idx_near][1] - cx;
        // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
        // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
        // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;

        // //考虑近点
        // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ;
        // //考虑远点
        pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
    }
}