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

#include "turn_a90.h"
#include "../Geometry.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../jni_log.h"

#include <vector>
#include <cstdlib>

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

using namespace std;

enum {
    TURN_ANGLE_90,
    TURN_ANGLE_90_CMP
};

const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool TA90Testing;
static bool carStopEvent;
static bool checked;
static int currTarget;
static int azimuth;
static uint32_t stopTimepoint = 0;

static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
static bool Turned(const Polygon *map, const car_model_cache_t *car);

void StartTurnA90(double heading)
{
    azimuth = (int) heading;
    checked = false;
    TA90Testing = true;
    carStopEvent = false;

    currTarget = TURN_ANGLE_90;
}

void StopTurnA90(void)
{
    TA90Testing = false;
}

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

    if (!TA90Testing)
        return -2;

    DEBUG("TestTurnA90 %d", run_status);

    if (CrashRedLine(map, car)) {
        // 压线了
        err.push_back(29);
        status = -1;
        DEBUG("错误 压线");
    }

    if (run_status != 0) {
        if (carStopEvent)
            DEBUG("TURN_ANGLE_90 停车时间 %ld", AppTimer_GetTickCount() - stopTimepoint);

        if (carStopEvent && AppTimer_GetTickCount() - stopTimepoint > STOP_CAR_TIME) {
            // 中途停车
            err.push_back(31);
            DEBUG("错误 停车1");
        }
        carStopEvent = false;
    } else if (!carStopEvent){
        carStopEvent = true;
        stopTimepoint = AppTimer_GetTickCount();
    }

    if (ExitTestArea(map, car)) {
        // 测试结束
        status = 1;
    }

    if (currTarget == TURN_ANGLE_90) {
        // 转向灯开启
        int az = (int) heading;

        if (abs(az - azimuth) > 180) {
            az = 360 - abs(az-azimuth);
        } else {
            az = abs(az - azimuth);
        }

        if (az >= 10 && !checked) {
            checked = true;
            // 转向灯未开启
            err.push_back(30);
            DEBUG("错误 灯没看");
        }

        if (Turned(map, car)) {
            currTarget = TURN_ANGLE_90_CMP;
            checked = false;
        }
        DEBUG("TURN_ANGLE_90 %d %d",run_status, az);
    } else {
        // 关闭转向灯
        Line line;

        MakeLine(&line, &map->point[1], &map->point[2]);

        // 大于2.5米后检查车灯
        if (!checked && DistanceOf(car->points[0], line) >= 2.5) {
            checked = true;
            // 转向灯未关闭
            err.push_back(30);
            DEBUG("错误 灯没管");
        }
        DEBUG("TURN_ANGLE_90_CMP");
    }

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

    return status;
}

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

    Line red_line;
    const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};

    Line frontAxle, rearAxle;

    // 仅看车轮外侧
    MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
        if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
            break;
        }
    }

    return ret;
}

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

static bool Turned(const Polygon *map, const car_model_cache_t *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(map->point[1], map->point[2], car->points[i]) != 1)
            return false;
    }
    return true;
}