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

#include <cstdlib>
#include <tuple>
#include <map>

#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../driver_test.h"
#include "../jni_log.h"
#include "park_bottom.h"
#include "uphill.h"
#include "park_edge.h"
#include "driving_curve.h"
#include "right_corner.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
#include "../common/observer.h"
#include "../native-lib.h"

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

enum class ProximityStatus {
    Near,
    Bounce,
    Far
};

ilovers::Observer<std::function<void(move_status_t, move_status_t, double)>> CarMoveEvent;
ilovers::Observer<std::function<void(int, int, double)>> ShiftEvent;

static void CheckProximity(prime_t  &prime, Line &base_line, Line &line, int this_id);
static bool CrossingStartLine(Line &trace, Line &start_line);
static void ProximityReminders(prime_t &prime);
static void DetectCarMove(prime_t &prime);
static void DetectShift(prime_t &prime);
static void EnterMap(prime_t &prime);
static void ExitMap(prime_t &prime);

void AreaExam(prime_t &prime)
{
    if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
        return;
    }
    DEBUG("--------------> %d", __LINE__);
    DetectShift(prime);
    DEBUG("--------------> %d", __LINE__);
    DetectCarMove(prime);
    DEBUG("--------------> %d", __LINE__);
    ProximityReminders(prime);
    DEBUG("--------------> %d", __LINE__);
    EnterMap(prime);
    DEBUG("--------------> %d", __LINE__);
    TestDrivingCurve(prime);
    DEBUG("--------------> %d", __LINE__);
    TestParkBottom(prime);
    DEBUG("--------------> %d", __LINE__);
    TestParkEdge(prime);
    DEBUG("--------------> %d", __LINE__);
    TestUphill(prime);
    DEBUG("--------------> %d", __LINE__);
    TestRightCorner(prime);
    DEBUG("--------------> %d", __LINE__);
    ExitMap(prime);
    DEBUG("--------------> %d", __LINE__);
}

// 检查换挡，如停车入库、倒车开始等动作，需要在挡位变换时检测，同时需考虑驾驶员一次换挡不进，停在原地再次操作的情况
static void DetectShift(prime_t &prime)
{
    static int prevGear = prime.sensor.gear;
    static double odo = prime.odo;          // 记录这个挡位下行驶的距离

    if (prime.sensor.gear != prevGear) {
        if (prime.odo - odo > 1) {

        }
        ShiftEvent.Notify(prime.sensor.gear, prevGear, prime.odo - odo);
        prevGear = prime.sensor.gear;
        odo = prime.odo;
    }
}

static bool stopTimeout = false;

static void StoppedTimeout(apptimer_var_t val) {
    stopTimeout = true;
}

// 处理各个项目的超时停车（停下时和再次启动时，挡位是一致的）
static void DetectCarMove(prime_t &prime)
{
    static double odo = prime.odo;
    static int gearWhenStop = GEAR_N;
    static move_status_t prevMove = prime.pMotion->move;
    if (prime.pMotion->move != prevMove) {
        // Notify
        DEBUG("行驶状态改变 %d", prime.pMotion->move);
        CarMoveEvent.Notify(prime.pMotion->move, prevMove, prime.odo - odo);

        AppTimer_delete(StoppedTimeout);
        if (prime.pMotion->move == STOP) {
            gearWhenStop = prime.sensor.gear;
            switch (prime.examing_area.type) {
                case MAP_TYPE_PARK_BUTTOM:
                    AppTimer_add(StoppedTimeout, prime.examParam.park_bottom_pause_criteria);
                    break;
                case MAP_TYPE_PARK_EDGE:
                    AppTimer_add(StoppedTimeout, prime.examParam.park_edge_pause_criteria);
                    break;
                case MAP_TYPE_RIGHT_CORNER:
                    AppTimer_add(StoppedTimeout, prime.examParam.turn_a90_pause_criteria);
                    break;
                case MAP_TYPE_CURVE:
                    AppTimer_add(StoppedTimeout, prime.examParam.curve_pause_criteria);
                    break;
                case MAP_TYPE_UPHILL:
                    break;
                default:break;
            }
        } else {
            if (stopTimeout && ((prime.sensor.gear == GEAR_R) == (gearWhenStop == GEAR_R))) {
                // 停车超时
                switch (prime.examing_area.type) {
                    case MAP_TYPE_PARK_BUTTOM:
                        // 停车超2秒，每次扣5分
                        AddExamFault(20106);
                        break;
                    case MAP_TYPE_PARK_EDGE:
                        AddExamFault(20406);
                        break;
                    case MAP_TYPE_RIGHT_CORNER:
                        AddExamFault(20703);
                        break;
                    case MAP_TYPE_CURVE:
                        // 停车超2秒，不合格
                        AddExamFault(20602);
                        break;
                    case MAP_TYPE_UPHILL:
                        break;
                    default:break;
                }
            }
        }
        stopTimeout = false;
        prevMove = prime.pMotion->move;
        odo = prime.odo;
    }
}

int RegisterCarMoveObserver(std::function<void(move_status_t, move_status_t, double)> ob)
{
    return CarMoveEvent.Connect(ob);
}

void UnregisterCarMoveObserver(int handle)
{
    CarMoveEvent.Disconnect(handle);
}

int RegisterShiftObserver(std::function<void(int, int, double)> ob)
{
    return ShiftEvent.Connect(ob);
}

void UnregisterShiftObserver(int handle)
{
    ShiftEvent.Disconnect(handle);
}

static void EnterMap(prime_t &prime)
{
    if (prime.examing_area.type != 0) {
        return;
    }

    DEBUG("--------------> %d", __LINE__);

    DEBUG("1. %d", prime.pModeling[prime.curr_modeling_index].points.size());
    DEBUG("2. %d", prime.pModeling[prime.prev_modeling_index].points.size());

    DEBUG("3. %d", prime.pModel->left_front_tire[TIRE_OUTSIDE]);
    DEBUG("4. %d", prime.pModel->right_front_tire[TIRE_OUTSIDE]);


    PointF lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
    PointF rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];

    Line left_trace, right_trace;
    DEBUG("--------------> %d", __LINE__);
    MAKE_LINE(left_trace, lp1, lp2);
    MAKE_LINE(right_trace, rp1, rp2);

    Line start_line;
    DEBUG("倒库> %d", std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
    for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.examing_area.type = MAP_TYPE_PARK_BUTTOM;
            prime.examing_area.idx = i;
            StartParkBottom(prime);
            return;
        }
        MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[7], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[6]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.examing_area.type = MAP_TYPE_PARK_BUTTOM;
            prime.examing_area.idx = i;
            StartParkBottom(prime);
            return;
        }
    }
    DEBUG("侧方> %d", std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size());
    for (int i = 0; i < std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.examing_area.type = MAP_TYPE_PARK_EDGE;
            prime.examing_area.idx = i;
            StartParkEdge(prime);
            return;
        }
    }
    DEBUG("上坡> %d", std::get<MAP_TYPE_UPHILL>(prime.maps).size());
    for (int i = 0; i < std::get<MAP_TYPE_UPHILL>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[0], std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[10]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.examing_area.type = MAP_TYPE_UPHILL;
            prime.examing_area.idx = i;
            StartUphill(prime);
            return;
        }
    }
    DEBUG("曲线> %d", std::get<MAP_TYPE_CURVE>(prime.maps).size());
    for (int i = 0; i < std::get<MAP_TYPE_CURVE>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].right_start_point, std::get<MAP_TYPE_CURVE>(prime.maps)[i].left_start_point);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.examing_area.type = MAP_TYPE_CURVE;
            prime.examing_area.idx = i;
            StartDrivingCurve(prime);
            return;
        }
    }
    DEBUG("直角> %d", std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size());
    for (int i = 0; i < std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[0], std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[1]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.examing_area.type = MAP_TYPE_RIGHT_CORNER;
            prime.examing_area.idx = i;
            StartRightCorner(prime);
            return;
        }
    }
}

/****************************************
 * 退出场地：条件是车头和四轮都不在场地中
 * 如果当前测试项目未完成，要报不合格
 * @param prime
 */
static void ExitMap(prime_t &prime)
{
    if (prime.examing_area.type == MAP_TYPE_NONE) {
        return;
    }

    if (prime.examing_area.type == MAP_TYPE_CURVE) {
        // 车的后轮是否越过结束线或是车辆不在2个大圆范围内
        PointF lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
        PointF lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
        PointF rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
        PointF rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];

        Line left_trace, right_trace;

        MAKE_LINE(left_trace, lp1, lp2);
        MAKE_LINE(right_trace, rp1, rp2);

        Line end_line;
        MAKE_LINE(end_line, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].right_end_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].left_end_point);
        if (CrossingStartLine(left_trace, end_line)) {
            // 离开区域
            StopDrivingCurve(prime);
        }

        if (DistanceOf(prime.pModeling[prime.curr_modeling_index].base_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius
            && DistanceOf(prime.pModeling[prime.curr_modeling_index].base_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {
            // 离开区域
            StopDrivingCurve(prime);
        }
    } else {
        MakePolygon area({});
        DEBUG("------------> %d", __LINE__);
        switch (prime.examing_area.type) {
            case MAP_TYPE_PARK_BUTTOM:
                area.AddPoints({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0],
                                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[2],
                                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[5],
                                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]});
                break;
            case MAP_TYPE_PARK_EDGE:
                area.AddPoints({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[0],
                                std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[1],
                                std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[2],
                                std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3],
                                std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4],
                                std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5],
                                std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[6],
                                std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[7]});
                break;
            case MAP_TYPE_UPHILL:
                area.AddPoints({std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[0],
                                std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[9],
                                std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[11],
                                std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[10]});
                break;
            case MAP_TYPE_RIGHT_CORNER:
                area.AddPoints({std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[0],
                                std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[1],
                                std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[2],
                                std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[3],
                                std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[4],
                                std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[5]});
                break;
            default:
                break;
        }

        DEBUG("------------> %d", __LINE__);
        int num = 0;

        if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                           area.GetPolygon()) == GM_None) {
            num++;
        }
        if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                           area.GetPolygon()) == GM_None) {
            num++;
        }
        if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                           area.GetPolygon()) == GM_None) {
            num++;
        }
        if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
                           area.GetPolygon()) == GM_None) {
            num++;
        }
        if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
                           area.GetPolygon()) == GM_None) {
            num++;
        }
        DEBUG("------------> %d", __LINE__);
        if (num == 5) {
            // 离开区域
            StopDrivingCurve(prime);
            StopParkBottom(prime);
            StopRightCorner(prime);
            StopUphill(prime);
            StopParkEdge(prime);
        }
        DEBUG("------------> %d", __LINE__);
    }
}

// 车轮驶过线，且车头位于右侧
static bool CrossingStartLine(Line &trace, Line &start_line)
{
    PointF head = trace.p1;

    if (IntersectionOf(trace, start_line) == GM_Intersection
        && IntersectionOfLine(head, start_line) == REL_POS_RIGHT) {
        return true;
    }
    return false;
}

static void ProximityReminders(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_NONE)
        return;

    DEBUG("----------------> %d", __LINE__);

    DEBUG("test %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
    DEBUG("test %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points.size());
    DEBUG("test %f,%f",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1].X, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1].Y);
    {
        Line test;

        MAKE_LINE(test, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[0]);

        DEBUG("test %f,%f - %f,%f",test.p1.X, test.p1.Y, test.p2.X, test.p2.Y);
    }

    DEBUG("----------------> %d", __LINE__);
    PointF car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
    PointF car_head_trend = PointExtend(car_head, 8, prime.pModeling->yaw);
    DEBUG("----------------> %d", __LINE__);

    Line car_head_line;
    MAKE_LINE(car_head_line, car_head, car_head_trend);
    DEBUG("----------------> %d", __LINE__);
    Line start_line;
    for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size(); ++i) {  // 左右2条控制线都可作为入口
        MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[0]);
        CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].id);
        MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[7], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[6]);
        CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].id);
    }
    for (int i = 0; i < std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[0]);
        CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].id);
    }

    for (int i = 0; i < std::get<MAP_TYPE_UPHILL>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[0], std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[10]);
        CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].id);
    }
    for (int i = 0; i < std::get<MAP_TYPE_CURVE>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].right_start_point, std::get<MAP_TYPE_CURVE>(prime.maps)[i].left_start_point);
        CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].id);
    }
    for (int i = 0; i < std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size(); ++i) {
        MAKE_LINE(start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[0], std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[1]);
        CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].id);
    }
}

// 判断是否接近场地的起始线
// 车头趋势线是否和入口线相交
static void CheckProximity(prime_t  &prime, Line &base_line, Line &line, int this_id)
{
    ProximityStatus status;

    if (CrossingStartLine(base_line, line)) {
        PointF p1 = base_line.p1;
        PointF p2 = base_line.p2;

        // 增加距离，用于消除测量误差带来的不稳定
        if (DistanceOf(p1, line) > 0.5 && DistanceOf(p2, line) > 0.5) {
            status = ProximityStatus::Near;
        } else {
            status = ProximityStatus::Bounce;
        }
    } else {
        status = ProximityStatus::Far;
    }

    if (status == ProximityStatus::Near && prime.arriving_map != this_id) {
        prime.arriving_map = this_id;
        // 报语音提示
        PlayTTS("即将考试", nullptr);
    } else if (status == ProximityStatus::Far && prime.arriving_map == this_id) {
        prime.arriving_map = -1;
    }
}

void TerminateAreaExam(void)
{

}

void InitAreaExam(void)
{
    ResetOdo();
}