//
// Created by YY on 2019/10/23.
//
//
// 0 ____________________________________7
//
//
// 1 _____________2     5_________________6
//              8|      |9
//               |      |
//               |      |
//               |      |
//              3--------4
//

#include "park_bottom.h"
#include "../common/apptimer.h"
#include "../test_common/Geometry.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../test_common/odo_graph.h"
#include "../teach/train.h"
#include "../teach/teach.h"
#include <vector>
#include <cstdlib>
#include <tuple>

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

using namespace std;

typedef enum {
    NONE_CTRL_LINE,
    LEFT_CTRL_LINE,
    RIGHT_CTRL_LINE
} ctrl_line_t;

static bool occurCrashRedLine;
static int parkCount;
static int reverseCnt = 0;
static bool stopFlag;
static ctrl_line_t prevCrossedCtrlLine;

static bool checkCtrlLine, checkParkspace;
static int handleGearOb, handleMoveOb;

static bool BodyCollidingLine(prime_t &prime);
static ctrl_line_t CrossCtrlLine(prime_t &prime);
static bool CheckParkspace(prime_t &prime);
static void MoveOb(move_status_t curr, move_status_t prev, double distance);
static void GearOb(int gear, int prevGear, double move_distance);

enum {
    PREPARE_REVERSE,
    REVERSE_LEFT,
    REVERSE_RIGHT,
    OBVERSE_LEFT,
    OBVERSE_RIGHT
};

/*
1. 进入 --- 首次倒库前
2. 倒库 ---- 停车到位前（右倒库）
3. 左出库
4. 倒库 ---- 停车到位前（左倒库）
5. 右出库(离开场地)
*/

void StartParkBottom(prime_t &prime)
{
    DEBUG("进入倒库场地");
    stopFlag = false;
    parkCount = 0;
    reverseCnt = 0;
    checkCtrlLine = false;
    checkParkspace = false;
    occurCrashRedLine = false;
    prime.examing_area.stage = PREPARE_REVERSE;
    testAll();
    LoadStageTips(prime);
    prevCrossedCtrlLine = NONE_CTRL_LINE;
    handleGearOb = RegisterShiftObserver(GearOb);
    handleMoveOb = RegisterCarMoveObserver(MoveOb);
    PlayTTS("您已进入倒车入库区域", NULL);
}

static void ParkTimeout(apptimer_var_t var)
{
    DEBUG("项目超时");
    AddExamFault(20105);
}

static void MoveOb(move_status_t curr, move_status_t prev, double distance)
{
    // 车辆由运动到停止，记录一下，当换挡发生时，避免原地来回换挡造成误判
    if (curr == STOP && distance > 0.5) {
        stopFlag = true;
    }
}

static void GearOb(int gear, int prevGear, double move_distance)
{
    DEBUG("GearOb %d %f", gear, move_distance);

    if (gear == GEAR_R && stopFlag) {   // 移入倒挡
        stopFlag = false;
        reverseCnt++;
        if (reverseCnt == 1) {
            AppTimer_delete(ParkTimeout);
            AppTimer_add(ParkTimeout, GetPrime().examParam.park_bottom_limit_time);
        }
        checkCtrlLine = true;
    } else if (prevGear == GEAR_R && stopFlag) {    // 从倒挡移出
        stopFlag = false;
        checkParkspace = true;
    }
}

void StopParkBottom(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
        return;

    DEBUG("离开倒库场地");

    if (parkCount < 2) {
        DEBUG("直接驶离测试区,不按考试员指令驾驶");
        AddExamFault(10103);
    }
    AppTimer_delete(ParkTimeout);
    prime.examing_area.type = MAP_TYPE_NONE;
    UnregisterShiftObserver(handleGearOb);
    UnregisterCarMoveObserver(handleMoveOb);
}

void TestParkBottom(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
        return;

    train(prime);

    if (checkCtrlLine) {
        ctrl_line_t crossCtrlLine = CrossCtrlLine(prime);

        if (crossCtrlLine == NONE_CTRL_LINE) {
            // 倒车前，前轮未驶过控制线
            DEBUG("倒车前，前轮未驶过控制线");
            AddExamFault(20104);
        } else if (crossCtrlLine == prevCrossedCtrlLine) {
            // 重复跨越同一控制线，不按规定线路，顺序形式，不合格
            AddExamFault(20101);
            DEBUG("不按规定线路，顺序形式， 同 %d 侧", prevCrossedCtrlLine);
        } else {
            prevCrossedCtrlLine = crossCtrlLine;
            DEBUG("开始 %d 侧 倒库", prevCrossedCtrlLine);
        }

        if (DeltaYaw(YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4]), prime.pModeling[prime.curr_modeling_index].yaw) < 90) {
            DEBUG("右侧倒库");
            prime.examing_area.stage = REVERSE_RIGHT;
        } else {
            DEBUG("左侧倒库");
            prime.examing_area.stage = REVERSE_LEFT;
        }
        LoadStageTips(prime);
        checkCtrlLine = false;
    }

    if (checkParkspace) {
        if (CheckParkspace(prime)) {
            DEBUG("倒库成功");
        } else {
            DEBUG("倒库不入");
            AddExamFault(20103);
        }
        parkCount++;
        if (prime.examing_area.stage == REVERSE_RIGHT) {
            prime.examing_area.stage = OBVERSE_LEFT;       // 从左侧倒库
        } else {
            prime.examing_area.stage = OBVERSE_RIGHT;      // 离开场地
        }
        LoadStageTips(prime);
        checkParkspace = false;
    }

    if (BodyCollidingLine(prime)) {
        if (!occurCrashRedLine) {
            occurCrashRedLine = true;
            // 车身出线，不合格
            AddExamFault(10116);
            DEBUG("车身出线");
        }
    } else {
        occurCrashRedLine = false;
    }

    teach(prime);
}

// 车身或车轮皆不得碰触实线
static bool BodyCollidingLine(prime_t &prime)
{
    MakePolygon car_body(prime.pModel->body.size());

    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
    }

    Line frontAxle, rearAxle;

    MAKE_LINE(frontAxle, 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]]);
    MAKE_LINE(rearAxle, 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]]);

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

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        Line red_line;
        MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
        if (IntersectionOf(red_line, frontAxle) == GM_Intersection
            || IntersectionOf(red_line, rearAxle) == GM_Intersection
            || IntersectionOf(red_line, car_body.GetPolygon()) != GM_None) {
            return true;
        }
    }
    return false;
}

// 检测2前轮是否正向越过左右控制线
static ctrl_line_t CrossCtrlLine(prime_t &prime)
{
    // 过右控制线
    Line left_trace, right_trace;
    Line left_ctrl, right_ctrl;

    MAKE_LINE(left_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);

    MAKE_LINE(left_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0]);
    MAKE_LINE(right_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[6], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[7]);


    if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection &&
            IntersectionOf(right_trace, left_ctrl) == GM_Intersection &&
            IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == REL_POS_LEFT) {
        return LEFT_CTRL_LINE;
    }

    if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection &&
        IntersectionOf(right_trace, right_ctrl) == GM_Intersection &&
        IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == REL_POS_RIGHT) {
        return RIGHT_CTRL_LINE;
    }

    return NONE_CTRL_LINE;
}

static bool CheckParkspace(prime_t &prime) {
    MakePolygon car_body(prime.pModel->body.size());

    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
    }

    MakePolygon park_area({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[8],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[9]});

    return (bool) (IntersectionOf(car_body.GetPolygon(), park_area.GetPolygon()) == GM_Containment);
}