//
// Created by YY on 2019/11/4.
//

#include "driving_curve.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include "../native-lib.h"
#include "area_exam.h"

#include <vector>
#include <cstdlib>
#include <tuple>

using namespace std;

#define DEBUG(fmt, args...)     LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)

static bool crashRedLine;

static bool CrashRedLine(prime_t &prime);

void StartDrivingCurve(prime_t &prime)
{
    DEBUG("进入曲线行驶场地");

    crashRedLine = false;

    MA_EnterMap(prime.examing_area.idx, MAP_TYPE_CURVE, 1);
}

void TestDrivingCurve(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_CURVE)
        return;

    if (CrashRedLine(prime)) {
        if (!crashRedLine) {
            // 车轮压边线，不合格
            DEBUG("车轮压边线");
            AddExamFault(20601);
        }
        crashRedLine = true;
    } else {
        crashRedLine = false;
    }
}

void StopDrivingCurve(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_CURVE)
        return;
    DEBUG("离开曲行驶是场地");

    prime.examing_area.type = MAP_TYPE_NONE;
}


// 曲线场地关键点
// 入口左右两点，出口左右两点，前半部大小圆圆心和直径，后半部大小圆圆心和直径，实际测量大小圆圆心偏差在20cm内
static bool CrashRedLine(prime_t &prime)
{
    // 判断是否在范围内，4个车轮只要满足在小圆外，大圆内，就说明没有压线
    PointF tires[] = {prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]};

    for (int i = 0; i < 4; i++) {
        if (IntersectionOfLine(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre,
                               std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) == REL_POS_RIGHT) {
            // 前半部分
            if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_radius
                && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius) {

            } else {
                return true;
            }
        } else {

            if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_radius
                && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {

            } else {
                return true;
            }
        }
    }

    return false;
}