//
// Created by YY on 2019/10/23.
//

#include "park_edge.h"
#include "../jni_log.h"
#include "../test_common/Geometry.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../native-lib.h"
#include "../utils/xconvert.h"
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"
#include "area_exam.h"

#include <vector>
#include <cstdlib>

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

using namespace std;

const int PARK_TIMEOUT = D_SEC(90);
const uint32_t STOP_CAR_TIME = D_SEC(2);
const uint32_t CHECK_PARK_DELAY = 400;

static int mapIndex = 0;
static bool reportExamTimeout;
static bool reportParkFail;

static uint32_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
static bool occurMoveBack, parkSuccess;
static uint32_t moveBackTimePoint;
static bool testing = false;

static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
static bool EnterParking(const Polygon *map, const car_model *car);
static bool ExitParkArea(const Polygon *map, const car_model *car);
static bool ExitParkArea2(const Polygon *map, const car_model *car);

void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime)
{
    DEBUG("进入侧方停车场地");

    testing = true;
    mapIndex = index;

    occurCrashRedLine1 = occurCrashRedLine2 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    reportExamTimeout = false;
    reportParkFail = false;
    prevMoveStatus = moveStatus;
    parkSuccess = false;
    parkStatus = 0;
    occurMoveBack = false;

    // 仅当发生倒车，才意味着项目开始
    if (moveStatus == -1) {
        occurMoveBack = true;
        moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }
}

int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
{
    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;

    if (CrashRedLine1(map, car)) {
        if (!occurCrashRedLine1 && occurMoveBack) {
            // 车轮压边线，每次扣10分
            AddExamFault(23, rtkTime);
            DEBUG("车轮压边线");
            occurCrashRedLine1 = true;
        }
    } else {
        occurCrashRedLine1 = false;
    }

    if (CrashRedLine2(map, car)) {
        if (!occurCrashRedLine2 && occurMoveBack) {
            // 车身压库位线，每次扣10分
            AddExamFault(24, rtkTime);
            DEBUG("车身压库位线");
            occurCrashRedLine2 = true;
        }
    } else {
        occurCrashRedLine2 = false;
    }

    if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
        if (!parkSuccess && occurMoveBack && !reportParkFail) {
            // 直接驶离测试区，认为移库不入
            AddExamFault(21, rtkTime);
            reportParkFail = true;
            DEBUG("直接驶离测试区,移库不入");
        }
        testing = false;
        goto TEST_END;
    }

    // 距离检测
    MakeLine(&distance_line, &map->point[0], &map->point[7]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[1], &map->point[2]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[2], &map->point[3]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[3], &map->point[4]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[4], &map->point[5]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[5], &map->point[6]);
    line_set.push_back(distance_line);
    DistanceOfTire2X(dtox, car, line_set);
    MA_SendDistance(dtox[0], dtox[1]);

    if (occurMoveBack) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (!reportExamTimeout && tp - moveBackTimePoint >= PARK_TIMEOUT) {
            // 超时90秒，不合格
            AddExamFault(22, rtkTime);
            reportExamTimeout = true;
            DEBUG("移库90秒超时");
        }
    }

    if (moveStatus != prevMoveStatus) {
        if (moveStatus == 0) {
            parkStatus = 0;
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            storeMoveStatusBeforeStop = prevMoveStatus;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
            uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

            DEBUG("停车时间 %ld", tp - stopTimepoint);

            if (moveStatus == storeMoveStatusBeforeStop) {
                // 同方向再启动，继续判断是否停车超时
                if (tp - stopTimepoint >= STOP_CAR_TIME && occurMoveBack) {
                    // 停车超2秒，每次扣5分
                    AddExamFault(26, rtkTime);
                    DEBUG("停车超时");
                }
            } else {
                // 倒车切换到前进
                if (moveStatus == 1 && tp - stopTimepoint >= CHECK_PARK_DELAY) {
                    if (parkStatus == 0) {
                        if (EnterParking(map, car)) {
                            parkStatus = 1;
                            parkSuccess = true;
                        }
                    }

                    if (!parkSuccess && !reportParkFail) {
                        // 停止后，车身出线，不合格
                        AddExamFault(21, rtkTime);
                        reportParkFail = true;
                        DEBUG("移库不入");
                    }

                    // 在这里检查转向灯状态
                    if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
                        // 不开转向灯，扣10分
                        AddExamFault(25, rtkTime);
                        DEBUG("未开启转向灯");
                    }
                }
            }

            if (moveStatus == -1 && !occurMoveBack) {
                DEBUG("开始倒车");
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
            }
        }

        prevMoveStatus = moveStatus;
    } else if (moveStatus == 0 && parkStatus == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
            if (EnterParking(map, car)) {
                parkStatus = 1;
                parkSuccess = true;
            } else {
                parkStatus = -1;
            }
        }
    }

TEST_END:
    if (!testing && occurMoveBack) {
        MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
    }
    return testing ? 1 : 0;
}

// 车轮是否压道路边线
static bool CrashRedLine1(const Polygon *map, const car_model *car)
{
    bool ret = false;

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

    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 < 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 CrashRedLine2(const Polygon *map, const car_model *car)
{
    bool ret = false;

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

    Polygon car_body;

    car_body.num = car->bodyNum;
    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = car->carXY[car->body[i]];
    }

    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, &car_body) != GM_None) {
            ret = true;
            break;
        }
    }

    free(car_body.point);
    return ret;
}

static bool EnterParking(const Polygon *map, const car_model *car) {
    DEBUG("检查停车到位...");

    bool succ = false;

    Polygon parking;
    Polygon car_body;

    car_body.num = car->bodyNum;
    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = car->carXY[car->body[i]];
    }

    MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});

    if (IntersectionOf(&car_body, &parking) == GM_Containment) {
        succ = true;
    }

    CleanPolygon(&parking);
    free(car_body.point);

    return succ;
}

// 整个车辆都要驶过前库位线
static bool ExitParkArea(const Polygon *map, const car_model *car)
{
    for (int i = 0; i < car->bodyNum; ++i) {
        if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->body[i]]) != -1)
            return false;
    }
    return true;
}

static bool ExitParkArea2(const Polygon *map, const car_model *car)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    }

    free(carBody.point);

    return ret;
}