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

enum cross_type_e cross_type = CROSS_NONE;
bool far_Lpt0_found, far_Lpt1_found;
int32_t Both_Boder_None_Cross;

void CheckCross() {
    bool Xfound = Lpt0_found && Lpt1_found;
    if (cross_type == CROSS_NONE && Xfound){ 
    cross_type = CROSS_BEGIN;
    }
}

void RunCross() {
    bool Xfound = Lpt0_found && Lpt1_found;
    aim_distance = 0.4;
    //检测到十字，先按照近线走
    if (cross_type == CROSS_BEGIN) {
        if (Lpt0_found) {
            mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
            track_type = TRACK_LEFT;
        }
        if (Lpt1_found) {
            mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id;
            track_type = TRACK_RIGHT;
        }

        //近角点过少，进入远线控制
        if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST || Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count <20 && pts_resample_right_count<20)) {
            cross_type = CROSS_IN;
            //cross_encoder = current_encoder;
        }
        
    }
        //远线控制进十字,begin_y渐变靠近防丢线
    else if (cross_type == CROSS_IN) {
        //寻远线,算法与近线相同
        cross_farline();

        if (pts_resample_left_count < 5 && pts_resample_right_count < 5) { 
            Both_Boder_None_Cross++;
            }
        if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) {
            cross_type = CROSS_NONE;
            Both_Boder_None_Cross = 0;
        }
        if (far_Lpt1_found) { track_type = TRACK_RIGHT; }
        else if (far_Lpt0_found) { track_type = TRACK_LEFT; }
        else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) { track_type = TRACK_RIGHT; }
        else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) { track_type = TRACK_LEFT; }

    }
}


void cross_farline(){}