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

#include "driving_curve.h"
#include "../driver_test.h"
#include "../common/apptimer.h"

#include <vector>

using namespace std;

enum {
    DRIVING_ON_CURVE
};

const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool carStopEvent = false;
static bool DCTesting = false;
static int currTarget;
static uint32_t stopTimepoint = 0;

static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car);

void StartDrivingCurve(void)
{
    DCTesting = true;
    carStopEvent = false;
    currTarget = DRIVING_ON_CURVE;
}

void StopDrivingCurve(void)
{
    DCTesting = false;
}

int TestDrivingCurve(vector<int>&err, const Polygon *map, const Polygon *map2, const car_model_cache_t *car, double speed, int run_status)
{
    int status = 0;

    if (!DCTesting)
        return -2;

    if (currTarget == DRIVING_ON_CURVE) {
        if (CrashRedLine(map, map2, car)) {
            // 车轮压线
            err.push_back(27);
            status = -1;
        }

        if (ExitTestArea(map, map2, car)) {
            // 测试完成
            status = 1;
        }

        if (run_status != 0) {
            if (carStopEvent && AppTimer_GetTickCount() - stopTimepoint > D_SEC(2)) {
                // 中途停车
                err.push_back(28);
            }
            carStopEvent = false;
        } else {
            carStopEvent = true;
            stopTimepoint = AppTimer_GetTickCount();
        }
    }

    if (status != 0) {
        StopDrivingCurve();
    }

    return status;
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
{
    bool ret = false;

    car_model_cache_t *prev_car = GetCarModelCache(1);
    if (prev_car == NULL)
        return false;

    // 按4个轮子的外侧计算
    Line front_left_tire_track, front_right_tire_track, rear_left_tire_track, rear_right_tire_track;
    MakeLine(&front_left_tire_track,  &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&front_right_tire_track,  &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_left_tire_track,  &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_right_tire_track,  &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);

    Line line;

    line.X1 = left->point[0].X;
    line.Y1 = left->point[0].Y;
    for (int i = 1; i < left->num; ++i) {
        line.X2 = left->point[i].X;
        line.Y2 = left->point[i].Y;

        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
    }

    line.X1 = right->point[0].X;
    line.Y1 = right->point[0].Y;
    for (int i = 1; !ret && i < right->num; ++i) {
        line.X2 = right->point[i].X;
        line.Y2 = right->point[i].Y;

        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
    }

    return ret;
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(left->point[0], right->point[0], car->points[i]) != 1)
            return false;
    }
    return true;
}