//
// Created by YY on 2020/3/17.
//

#include "road_exam.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../jni_log.h"

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

static bool occurCrashRedLine;
static bool occurCrashGreenLine;
static bool occurOverSpeed;
static int checkCrashGreenTimeout;
static char carIntersectionOfGreenLine;

struct {
    int hour;
    int min;
    int sec;
    int msec;
} crashGreenRunTime, crashGreenCmpTime;

static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;

static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2);

void Init(void)
{
    crashGreenCmpTime.hour = -1;
    occurCrashRedLine = false;
    occurCrashGreenLine = false;
    occurOverSpeed = false;
    checkCrashGreenTimeout = 0;
    carIntersectionOfGreenLine = 0;
}

void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, double speed, int moveStatus, const struct RtkTime *rtkTime)
{
    if (speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
            AddExamFault(10, rtkTime);
        }
    } else {
        occurOverSpeed = false;
    }

    if (CrashRedLine(RoadMapList, car)) {
        if (!occurCrashRedLine) {
            // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
            AddExamFault(11, rtkTime);
            occurCrashRedLine = true;
        }
    } else {
        occurCrashRedLine = false;
    }

    static PointF p1, p2;

    if (CrashGreenLine(RoadMapList, car, p1, p2)) {
        if (moveStatus != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                crashGreenRunTime.hour = rtkTime->hh;
                crashGreenRunTime.min = rtkTime->mm;
                crashGreenRunTime.sec = rtkTime->ss;
                crashGreenRunTime.msec = rtkTime->mss;
            } else if (checkCrashGreenTimeout == 1) {
                uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                        crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);

                if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶，不合格
                    AddExamFault(12, rtkTime);
                }
            }
        } else {
            // 停车就不计时了
            checkCrashGreenTimeout = 0;
        }

        // 检测当前车辆于虚线的位置，做变道检测;
        // 检测是否3秒前有开启对应之转向灯
        if (!occurCrashGreenLine) {
            occurCrashGreenLine = true;

        }

        // p1 ---------------> p2
        double angle = car->yaw - YawOf(p2, p1);
        if (angle < 0 || angle > 180) {
            // 右侧
            carIntersectionOfGreenLine = 'R';
        } else {
            // 左侧
            carIntersectionOfGreenLine = 'L';
        }
    } else {
        if (occurCrashGreenLine) {
            int inter = IntersectionOfLine(p1, p2, car->basePoint);

            // 有过线动作
            if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
                    (inter == -1 && carIntersectionOfGreenLine == 'L')) {
                DEBUG("跨越虚线");

                if (crashGreenCmpTime.hour >= 0) {
                    uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                rtkTime->mss * 10,
                                                crashGreenCmpTime.hour, crashGreenCmpTime.min,
                                                crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10);

                    if (diff < CHANGE_ROAD_MIN_INTERVAL) {
                        // 连续变道，不合格
                        AddExamFault(15, rtkTime);
                    }
                }

                crashGreenCmpTime.hour = rtkTime->hh;
                crashGreenCmpTime.min = rtkTime->mm;
                crashGreenCmpTime.sec = rtkTime->ss;
                crashGreenCmpTime.msec = rtkTime->mss;
            }
        }
        occurCrashGreenLine = false;
        checkCrashGreenTimeout = 0;
    }
}

/*****************************************************
 * CrashRedLine 整个考试区域的道路边缘线，实线等。
 * 按车轮碰触计算，车身不计
 */
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car)
{
    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    for (int i = 0; i < RoadMapList.size(); ++i) {
        if (RoadMapList[i].type == 100) {

            // 每条线都检测
            for (int j = 0; j < RoadMapList[i].redLineNum; ++j) {
                Line red_line;
                int kp = 0;

                for (int k = 1; k < RoadMapList[i].redLine[j].num; ++k) {
                    MakeLine(&red_line, &RoadMapList[i].redLine[j].point[kp], &RoadMapList[i].redLine[j].point[k]);

                    if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
                        IntersectionOf(red_line, rearAxle) == GM_Intersection) {
                        return true;
                    }
                    kp = k;
                }
            }
            break;
        }
    }
    return false;
}

/**************************************************
 * 车轮触碰道路虚线。检测行驶时间超长；变道情况；
 * @param RoadMapList
 * @param car
 */
static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2)
{
    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    for (int i = 0; i < RoadMapList.size(); ++i) {
        if (RoadMapList[i].type == 100) {

            // 每条线都检测
            for (int j = 0; j < RoadMapList[i].greenLineNum; ++j) {
                Line green_line;
                int kp = 0;

                for (int k = 1; k < RoadMapList[i].greenLine[j].num; ++k) {
                    MakeLine(&green_line, &RoadMapList[i].greenLine[j].point[kp], &RoadMapList[i].greenLine[j].point[k]);

                    if (IntersectionOf(green_line, frontAxle) == GM_Intersection ||
                        IntersectionOf(green_line, rearAxle) == GM_Intersection) {

                        p1 = RoadMapList[i].greenLine[j].point[kp];
                        p2 = RoadMapList[i].greenLine[j].point[k];

                        return true;
                    }
                    kp = k;
                }
            }
            break;
        }
    }
    return false;
}