#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 aim_distance_select(void)
{
    if (cross_type != CROSS_NONE) {
        aim_distance = cricle_aim;
    } else if (circle_type != CIRCLE_NONE) {
        aim_distance = cross_aim;
    } else if (barrier_type != BARRIER_NONE) {
        aim_distance = barrier_aim;
    } else {
        aim_distance = common_aim;
    }
}

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

    if (garage_type != GARAGE_NONE) {
        cross_type  = CROSS_NONE;
        circle_type = CIRCLE_NONE;
    }
    if (cross_type != CROSS_NONE) {
        circle_type  = CIRCLE_NONE;
        barrier_type = BARRIER_NONE;
    }
    if (barrier_type != BARRIER_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();
    } else if (barrier_type != BARRIER_NONE) {
        RunBarrier();
    }
}

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);

        int gap_1 = fabs(rptsn[3 * (rptsn_num / 4)][1] - rptsn[0][1]);
        float dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx][0];
        float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1];
        float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
        float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0];
        float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1];
        float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2);
        float c1 = dx1 / dn1;
        float s1 = dy1 / dn1;
        float c2 = dx2 / dn2;
        float s2 = dy2 / dn2;
        float angle_1= atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
        ips114_show_float(120, 80, angle_1, 3, 4);
        if (-0.3 <= angle_1 <= 0.3 )
        {
            state_type = TURN_STATE;
        }
        else
        {
            state_type = STRAIGHT_STATE;
        }
        if (circle_type == CIRCLE_LEFT_IN && circle_type == CIRCLE_LEFT_OUT && circle_type == CIRCLE_RIGHT_IN && circle_type == CIRCLE_RIGHT_OUT)
        {
            state_type = TURN_STATE;
        }
        

        // 计算远锚点偏差值
        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;

        if (barrier_type == BARRIER_LEFT_BEGIN) {
            dx_near    = rptsn[aim_idx_near][1] - cx +barrier_offset;
            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
        } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
            dx_near    = rptsn[aim_idx_near][1] - cx - barrier_offset;
            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
        } else {
            dx_near    = rptsn[aim_idx_near][1] - cx;
            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
        }

        // // 计算近锚点偏差值
        // 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;
    }
}