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

#include "park_bottom.h"
#include "../common/apptimer.h"
#include "../Geometry.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include <vector>

using namespace std;

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

const int PARK_TIMEOUT = 210;

static bool PBTesting = false;
static bool trigLeaveTestAreaDetect = false;
static bool leaveTestArea = false;
static bool stopCar2S = false;

static int currTarget;
static bool leftTireCrossLeftLine, leftTireCrossRightLine, rightTireCrossLeftLine, rightTireCrossRightLine;
static char first_ctrl_line_id;
static bool carStopEvent;               // 中途停车标记
static bool carParkSuccess;             // 是否停在库位
static bool parkTimeout;

static void StopCarTimeout(union sigval sig);
static void LeaveTestAreaLongtime(union sigval sig);
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car);
static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
static void ParkBottomTimeout(union sigval sig);
static bool CrashRedLine(const Polygon *map, const car_model_cache_t *car);

void StartParkBottom(void)
{
    stopCar2S = false;
    trigLeaveTestAreaDetect = false;
    leaveTestArea = false;
    PBTesting = true;
    parkTimeout = false;
    first_ctrl_line_id = 0;
    currTarget = FIRST_TOUCH_CTRL_LINE;
    leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

    TextOsd(0, "ParkBottom");
}

void StopParkBottom(void)
{
    PBTesting = false;
    AppTimer_delete(StopCarTimeout);
    AppTimer_delete(ParkBottomTimeout);
    AppTimer_delete(LeaveTestAreaLongtime);
    currTarget = NONE;

    TextOsd(0, "ParkBottom End");
}

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

    if (!PBTesting)
        return -2;

    DEBUG("TestParkBottom speed %f dir %d", speed, run_status);


    if (currTarget > FIRST_TOUCH_CTRL_LINE) {
        // 是否超时
        if (parkTimeout) {
            // 不合格：动作超时
            err.push_back(10);
            status = -1;
        }
        // 是否压线
        if (CrashRedLine(map, car)) {
            // 不合格：车身出线
            err.push_back(7);
            status = -1;
        }

        if (trigLeaveTestAreaDetect) {
            if (IntersectionOf(car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], map) == GM_Containment &&
                IntersectionOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], map) == GM_Containment) {
                trigLeaveTestAreaDetect = false;
                AppTimer_delete(LeaveTestAreaLongtime);
            }
        }
    }

    if (currTarget == FIRST_TOUCH_CTRL_LINE ||
        currTarget == SECOND_TOUCH_CTRL_LINE ||
        currTarget == THIRD_TOUCH_CTRL_LINE) {
        if (run_status > 0) {
            if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
                // 扣5分：中途停车超过2秒
                err.push_back(11);
            }

            if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
                    (leftTireCrossRightLine && rightTireCrossRightLine))) {
                DEBUG("CrossCtrlLine");
                CrossCtrlLine(map, car);
            } else if (currTarget == FIRST_TOUCH_CTRL_LINE || currTarget == SECOND_TOUCH_CTRL_LINE) {
                // 跨过控制线后，车辆持续向前行驶，处理这些乱搞情况
                // 整个车都离开测试区后，如果持续15秒，还没回到测试区，就忽略该测试或者淘汰
                if (leaveTestArea) {
                    if (currTarget == FIRST_TOUCH_CTRL_LINE) {
                        status = -2;
                        TextOsd(0, "来道场玩的");
                        DEBUG("来道场玩的");
                    } else {
                        // 不合格：未按规定线路行驶（直接跑出测试区了）
                        err.push_back(6);
                        status = -1;
                        TextOsd(0, "直接跑出测试区了");
                        DEBUG("直接跑出测试区了");
                    }
                } else if (!trigLeaveTestAreaDetect) {
                    Polygon car_body;

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

                    if (IntersectionOf(map, &car_body) == GM_None) {
                        trigLeaveTestAreaDetect = true;
                        AppTimer_delete(LeaveTestAreaLongtime);
                        AppTimer_add(LeaveTestAreaLongtime, D_SEC(15));
                        DEBUG("开始离场计时");
                    }

                    free(car_body.point);
                }
            }

            if (currTarget == THIRD_TOUCH_CTRL_LINE) {
                char the_ctrl_line_crossed = 0;

                if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
                    the_ctrl_line_crossed = 'L';
                } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
                    the_ctrl_line_crossed = 'R';
                }

                if (the_ctrl_line_crossed != 0 && the_ctrl_line_crossed == first_ctrl_line_id) {
                    // 项目完成
                    status = 1;
                } else if (the_ctrl_line_crossed != 0) {
                    // 不合格：未按规定线路行驶（未回到起始点）
                    err.push_back(6);
                    status = -1;
                }
            }
            if (carStopEvent)
                AppTimer_delete(StopCarTimeout);
            carStopEvent = false;
            stopCar2S = false;
        } else if (run_status < 0) {
            // 左右倒库大纲并未要求谁先完成，故以先越过的控制线为准，下次得越过另外一条
            char the_ctrl_line_crossed = 0;

            if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
                the_ctrl_line_crossed = 'L';
            } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
                the_ctrl_line_crossed = 'R';
            }

            if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
                // 不合格：未按规定线路行驶（试图做2次同方向的倒库）
                err.push_back(6);
                status = -1;
            } else if (the_ctrl_line_crossed > 0 && first_ctrl_line_id == 0) {
                first_ctrl_line_id = the_ctrl_line_crossed;
                // 项目正式开始，210秒内完成
                AppTimer_delete(ParkBottomTimeout);
                AppTimer_add(ParkBottomTimeout, D_SEC(PARK_TIMEOUT));
                currTarget = FIRST_PARK;
                carParkSuccess = false;
                parkTimeout = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

                TextOsd(0, "第一次倒库");
            } else if (the_ctrl_line_crossed > 0) {
                currTarget = SECOND_PARK;
                carParkSuccess = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
                TextOsd(0, "第二次倒库");
            } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
                // 不合格：倒车前，2前轮没驶过控制线
                err.push_back(9);
                status = -1;
            }
            if (carStopEvent)
                AppTimer_delete(StopCarTimeout);
            carStopEvent = false;
            stopCar2S = false;
        } else {
            if (!carStopEvent) {
                AppTimer_delete(StopCarTimeout);
                AppTimer_add(StopCarTimeout, D_SEC(2));
            }
            carStopEvent = true;
        }
    } else if (currTarget == FIRST_PARK || currTarget == SECOND_PARK) {
        if (run_status < 0) {
            if (stopCar2S) {
                // 扣5分：中途停车
                err.push_back(11);
            }

            carStopEvent = false;
            stopCar2S = false;
        } else if (run_status == 0) {
            // 立即检查是否停车到位，也许是中途停车，先不管，待发生前进事件后，再断定是否停车到位
            if (!carStopEvent) {
                carStopEvent = true;
                carParkSuccess = EnterParking(map, car);
                AppTimer_delete(StopCarTimeout);
                AppTimer_add(StopCarTimeout, D_SEC(2));
            }
        } else {
            if (carStopEvent) {
                if (!carParkSuccess) {
                    // 不合格：倒库不入
                    err.push_back(8);
                    status = -1;
                } else if (currTarget == FIRST_PARK) {
                    currTarget = SECOND_TOUCH_CTRL_LINE;
                    TextOsd(0, "过另一根控制线");
                } else {
                    currTarget = THIRD_TOUCH_CTRL_LINE;
                    TextOsd(0, "再过第一根控制线");
                }
            }
            carStopEvent = false;
            stopCar2S = false;
        }
    }

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

    return status;
}

static void StopCarTimeout(union sigval sig) {
    AppTimer_delete(StopCarTimeout);

    stopCar2S = true;
}

static void LeaveTestAreaLongtime(union sigval sig) {
    AppTimer_delete(LeaveTestAreaLongtime);
    leaveTestArea = true;
    trigLeaveTestAreaDetect = false;
}

static void ParkBottomTimeout(union sigval sig) {
    AppTimer_delete(ParkBottomTimeout);
    parkTimeout = true;
}

// 检测2前轮是否正向越过左右控制线
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car)
{
    Line leftCtrlLine, rightCtrlLine;
    Line track1;
    PointF p1, p2;

    car_model_cache_t *prev_car = GetCarModelCache(1);
    if (prev_car == NULL)
        return;

    MakeLine(&leftCtrlLine, &map->point[0], &map->point[1]);
    MakeLine(&rightCtrlLine, &map->point[6], &map->point[7]);

    // 左前轮，取轮宽的中点
    p1.X = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;

    p2.X = (prev_car->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;

    MakeLine(&track1, &p1, &p2);
    
    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
        IntersectionOf(p1, map) == GM_None) {
        leftTireCrossLeftLine = true;
    }

    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
        IntersectionOf(p1, map) == GM_None) {
        leftTireCrossRightLine = true;
    }

    // 右前轮
    p1.X = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;

    p2.X = (prev_car->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;

    MakeLine(&track1, &p1, &p2);

    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
        IntersectionOf(p1, map) == GM_None) {
        rightTireCrossLeftLine = true;
    }
    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
        IntersectionOf(p1, map) == GM_None) {
        rightTireCrossRightLine = true;
    }
}

static bool EnterParking(const Polygon *map, const car_model_cache_t *car) {
    bool succ = false;

    Polygon parking;
    Polygon car_body;

    car_body.num = car->desc->body_num;
    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = car->points[car->desc->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 CrashRedLine(const Polygon *map, const car_model_cache_t *car)
{
    bool ret = false;

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

    Polygon car_body;

    car_body.num = car->desc->body_num;
    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = car->points[car->desc->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;
}