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



void get_corners() {
    Lpt0_found = Lpt1_found = false;
    Lpt_in0_found = Lpt_in1_found = false;
    is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST;
    is_straight1 = pts_resample_right_count > 1.0 / RESAMPLEDIST;
    for (int i = 0; i < pts_resample_left_count; i++) {
        if (angle_new_left[i] == 0) continue;
        int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
        float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2;

        //L角点阈值
        if (Lpt0_found == false && (66. / 180. * PI) < conf && conf < (140. / 180. * PI) && i < 0.5 / RESAMPLEDIST) {
            Lpt0_rpts0s_id = i;
            Lpt0_found = true;
            transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]);    //待优化，是否用到，无用则删
        }
        //长直道阈值
        if (conf > (7. / 180. * PI) && i < 0.8 / RESAMPLEDIST) is_straight0 = false;
        if (Lpt0_found == true && is_straight0 == false) break;
    }
    // if(Lpt0_found){
    //     float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],pts_resample_left[Lpt0_rpts0s_id+5][1],pts_resample_left[Lpt0_rpts0s_id+5][0]);
    //     if(angle1 < 85.)    {
    //         Lpt_in0_found = true;
    //         Lpt0_found = false;
    //         Lpt_in0_rpts0s_id = Lpt0_rpts0s_id;
    //     }
    // }
    // else{
    //     is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05);
    //     if(is_turn0){
    //         state_type = TURN_STATE;
    //     }
    // }
    if(is_straight0){
        state_type = STRAIGHT_STATE;
    }




    for (int i = 0; i < pts_resample_right_count; i++) {
        if (angle_new_right[i] == 0) continue;
        int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
        float conf = fabs(angle_right[i]) - (fabs(angle_right[im1]) + fabs(angle_right[ip1])) / 2;


        if (Lpt1_found == false && (66. / 180. * PI) < conf && conf < 140. / 180. * PI && i < 0.5 / RESAMPLEDIST) {
            Lpt1_rpts1s_id = i;
            Lpt1_found = true;
            transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]);
        }

        if (conf > (7. / 180. * PI) && i < 0.8 / RESAMPLEDIST) is_straight1 = false;
        if (Lpt1_found == true && is_straight1 == false) break;
    }
    // if(Lpt1_found){
    //     float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],pts_resample_right[Lpt1_rpts1s_id+5][1],pts_resample_right[Lpt1_rpts1s_id+5][0]);
    //     if(angle2 > 100.)   {
    //         Lpt_in1_found = true;
    //         Lpt1_found = false;
    //         Lpt_in1_rpts1s_id = Lpt1_rpts1s_id;
    //     }
    // }
    //else{
    //     is_turn1 = is_curve(angle_right, clip(angle_right_num - 10,0 ,angle_right_num), 0.05);
    //     if(is_turn1){
    //         state_type = TURN_STATE;
    //     }
    //}
    if(is_straight1){
        state_type = STRAIGHT_STATE;
    }
    

}