//
// 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 <vector>
#include <cstdlib>

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

using namespace std;

enum {
    NONE,
    FIRST_TOUCH_CTRL_LINE,
    FIRST_PARK,
    SECOND_TOUCH_CTRL_LINE,
    SECOND_PARK,
    THIRD_TOUCH_CTRL_LINE
};

enum {
    TESTING,
    TEST_FAIL,          // 因触发某些规则，在车身未完全立场情况下，提前终止部分测试
    TEST_FINISH
};

typedef enum {
    NONE_CTRL_LINE,
    LEFT_CTRL_LINE,
    RIGHT_CTRL_LINE
} ctrl_line_t;

static bool reverseCar = false;

const uint32_t CHECK_PARK_DELAY = 400;

static uint32_t stopTimepoint;
static int prevMoveDirect;
static bool occurCrashRedLine;

static uint32_t firstReverseTimepoint;

static int parkCount;
static char carray[3];
static int darray[3];
static int parkStatus[3];

static int gearAtStop;
static int currGear;
static ctrl_line_t prevCrossedCtrlLine;
static double odo;

static ctrl_line_t CrossCtrlLine(prime_t &prime);
static bool EnterParking(prime_t &prime);
static bool CrashRedLine(prime_t &prime);


void StartParkBottom(prime_t &prime)
{
    DEBUG("StartParkBottom");

    reverseCar = false;
    memset(carray, 0, sizeof(carray));
    memset(darray, 0, sizeof(darray));
    memset(parkStatus, 0, sizeof(parkStatus));
    prevMoveDirect = prime.pMotion->move;

    firstReverseTimepoint = 0;

    parkCount = 0;

    occurCrashRedLine = false;

    currGear = ReadCarStatus(GEAR);
    prevCrossedCtrlLine = NONE_CTRL_LINE;
    stopTimepoint = 0;

    odo = ReadOdo();

    PlayTTS("您已进入倒车入库区域", NULL);
}

void TestParkBottom(prime_t &prime)
{
    ctrl_line_t crossCtrlLine = NONE_CTRL_LINE;
    uint32_t tp = AppTimer_GetTickCount();

    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;

    bool gear_change = false;
    int gear = ReadCarStatus(GEAR);

    if (gear == GEAR_R) {
        if (currGear != GEAR_R) {
            gear_change = true;
            currGear = GEAR_R;
        }
    } else if (currGear == GEAR_R) {
        gear_change = true;
        currGear = gear;
    }

    if (gear_change) {
        // 检查上一次挡位的行驶距离，过小就放弃，避开学员原地挂挡重试
        double run_distance = ReadOdo() - odo;

        DEBUG("2次挡位运行距离 %f", run_distance);

        if (run_distance < 1) {
            gear_change = false;
            DEBUG("2次挡位运行距离过小，忽略");
        }
        odo = ReadOdo();
    }

    if (gear_change) {
        if (currGear == GEAR_R) {
            // 挂倒挡,检测是否过控制线
            DEBUG("开始挂倒挡");
            if (!reverseCar) {
                DEBUG("开始首轮入库");
                reverseCar = true;
                MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }
            crossCtrlLine = CrossCtrlLine(prime);

            if (parkCount >= 2) {
                DEBUG("开始次轮入库");
                parkCount = 0;
                MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }

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

            parkCount++;

            DEBUG("库位检查次数 = %d", parkCount);

            if (EnterParking(prime)) {
                DEBUG("倒库成功");
            } else {
                AddExamFault(20103);
                DEBUG("倒库不入");
            }
        }
    }

    if (ExitParkArea(prime)) {
        // 离开场地
        DEBUG("离开场地");
        if (parkCount < 2) {
            AddExamFault(10103);
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
        }
        prime.curr_exam_map.type = 0;
        goto TEST_END;
    }

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

    if (prime.pMotion->move != prevMoveDirect) {
        if (prime.pMotion->move == STOP) {
            stopTimepoint = tp;
            gearAtStop = (currGear == GEAR_R ? 1 : 0);
            DEBUG("停车了");
            DEBUG("停车时挡位 = %d", gearAtStop);
        } else if (prevMoveDirect == STOP && stopTimepoint > 0) {
            DEBUG("继续行驶");

            DEBUG("停车时间 %ld", tp - stopTimepoint);
            DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0);

            if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
                && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
                // 停车超2秒，每次扣5分
                AddExamFault(20106);
                DEBUG("中途停车");
            }
        }

        prevMoveDirect = prime.pMotion->move;
    }
TEST_END:
    DEBUG("倒库结束");
    MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 0);
}

static int BodyCollidingLine(prime_t &prime)
{
    vector<Line> lines;

    Polygon car_body;

    car_body.num = prime.pModel->body.size();
    car_body.point = new PointF[car_body.num];

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

    Line line;
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6]);
    lines.push_back(line);

    int idx = 0;

    for (auto line: lines) {
        if (IntersectionOf(line, &car_body) != GM_None) {
            break;
        }
        idx++;
    }
    delete []car_body.point;
    return idx < lines.size()? idx : -1;
}

// 检测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, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0]);
    MAKE_LINE(right_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[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) == RELATION_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) == RELATION_RIGHT) {
        return RIGHT_CTRL_LINE;
    }

    return NONE_CTRL_LINE;
}

// 需要库入口线宽计算在内
static bool EnterParking(prime_t &prime) {
    bool succ = false;

    Polygon park_area;
    Polygon car_body;

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

    PointF p8 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
                prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
                YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]));

    PointF p9 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
                prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
                YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]));

    MakePolygon(&park_area, {p8, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], p9});

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

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

    DEBUG("检查倒库状态 %s", succ ? "成功" : "失败");

    return succ;
}

// 4个车轮和车头点不在场地中
bool ExitParkArea(prime_t &prime)
{
    Polygon polygon;

    polygon.num = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map.size();
    polygon.point = new PointF[polygon.num];
    for (int i = 0; i < polygon.num; ++i) {
        polygon.point[i] = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[i];
    }

    int num = 0;

    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
                       &polygon) == GM_None) {
        num++;
    }

    delete []polygon.point;

    return num == 5? true : false;
}