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

//      11                 |9
//                         |
//                         |
//                         |
//                         |
//       7-----------------|8
//       6_________________|5
//       3_________________|4
//                         |
//       2-----------------|1
//                         |
//                         |
//                         |
//       10                |0

#include <cstdlib>
#include <vector>
#include <cmath>
#include <tuple>

#include "uphill.h"
#include "../driver_test.h"
#include "../jni_log.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"
#include "area_exam.h"
#include "../defs.h"

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

using namespace std;

const double EPSILON = 1e-3;

static int handle;
static PointF stopPoint;

static bool check1 = false;

static bool stopConfirm;
static bool restartComplete = false;
static bool occurCrashRedLine = false;

static bool slideNormalDistance = false;
static bool reportSlideFault = false;
static bool handBreakActive = false;

static bool CrashRedLine(prime_t &prime);
static double DistanceOfHead2Stopline(prime_t &prime);
static double DistanceOfTire2Edge(prime_t &prime);
static bool ExitTestArea(prime_t &prime);
static void MotionChange(move_status_t mv, move_status_t prev, double distance);

void StartUphill(prime_t &prime)
{
    DEBUG("进入坡起项目");

    stopConfirm = false;
    restartComplete = false;
    occurCrashRedLine = false;
    check1 = false;
    slideNormalDistance = false;
    reportSlideFault = false;
    handBreakActive = false;
    handle = RegisterCarMoveObserver(MotionChange);
    MA_EnterMap(prime.examing_area.idx, MAP_TYPE_UPHILL, 1);
}

void StopUphill(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_UPHILL)
        return;
    DEBUG("退出坡起项目");

    if (!check1) {
        DEBUG("未进行停车起步动作");
        AddExamFault(10103);
    }

    UnregisterCarMoveObserver(handle);

    prime.examing_area.type = MAP_TYPE_NONE;
}

static void StoppingTimeout(apptimer_var_t val) {
    // 起步时间超过30秒，不合格
    AddExamFault(20303);
    DEBUG("起步时间超过30秒");
}

static void StopConfirm(apptimer_var_t val) {
    stopConfirm = true;
}

static void MotionChange(move_status_t mv, move_status_t prev, double distance)
{
    AppTimer_delete(StopConfirm);

    if (mv == STOP) {
        AppTimer_add(StopConfirm, D_SEC(1));
    } else {

    }
}

void TestUphill(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_UPHILL)
        return;
    static double distanceToStopLine = 0, distanceToEdge = 0;

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

    // 检测到停车
    if (prime.pMotion->move == STOP && stopConfirm) {
        if (!check1) {
            check1 = true;
            // 存储停止点
            stopPoint = prime.pModeling->base_point;
            // 开始停车计时
            AppTimer_delete(StoppingTimeout);
            AppTimer_add(StoppingTimeout, prime.examParam.ramp_start_car_limit_time);

            // 检查车头和停车带的距离
            distanceToStopLine = DistanceOfHead2Stopline(prime);
            distanceToEdge = DistanceOfTire2Edge(prime);

            DEBUG("DIS1 = %f  DIS2 = %f", distanceToStopLine, distanceToEdge);

            if (distanceToStopLine > prime.examParam.ramp_stoppoint_red_distance) {
                // 距离停止线前后超出50厘米
                AddExamFault(20301);
                DEBUG("距离停止线前后超出50厘米，不合格");
            } else if (fabs(distanceToStopLine) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(20304);
                DEBUG("前保险没有位于停止带内，但没有超出50厘米");
            }

            if (distanceToEdge > prime.examParam.ramp_edge_red_distance) {
                // 距离边线超出50厘米,不合格
                AddExamFault(20302);
                DEBUG("距离边线超出50厘米");
            } else if (distanceToEdge > prime.examParam.ramp_edge_yellow_distance) {
                // 距离边线超出30厘米，扣10分
                AddExamFault(20305);
                DEBUG("距离边线超出30厘米");
            }
        }

        // 停车后，检查手刹拉起情况
        if (!handBreakActive && prime.sensor.hand_brake == ACTIVE) {
            handBreakActive = true;
        }
    }

    if (prime.pMotion->move != STOP && stopConfirm) {
        // 车辆从停止状态再次移动
        double juli = DistanceOf(prime.pModeling->base_point, stopPoint);

        if (juli > prime.examParam.ramp_slide_yellow_distance) {
            double deg = YawOf(stopPoint, prime.pModeling->base_point);
            deg = fabs(prime.pModeling->yaw - deg);
            if (deg > 180) {
                deg = 360 - deg;
            }
            if (deg < 90) {
                // 车辆上坡
                if (!restartComplete) {
                    restartComplete = true;
                    AppTimer_delete(StoppingTimeout);
                    if (slideNormalDistance && !reportSlideFault) {
                        // 后滑超过10厘米，但没超过30厘米
                        DEBUG("后滑超过10厘米，但没超过30厘米");
                        reportSlideFault = true;
                        AddExamFault(10204);
                    }
                    if (!handBreakActive) {
                        // 检查是否拉住手刹
                        DEBUG("没拉手刹");
                        handBreakActive = true;
                        AddExamFault(20306);
                    }
                }
            } else {
                // 车辆后滑
                slideNormalDistance = true;
                if (juli > prime.examParam.ramp_slide_red_distance && !reportSlideFault) {
                    // 后滑超过30厘米, 不合格
                    DEBUG("后滑超过30厘米");
                    reportSlideFault = true;
                    AddExamFault(10106);
                }
            }
        }
    }

    if (ExitTestArea(prime)) {
        // 驶离测试区
        if (!stopConfirm) {
            // 不停车直接离开
            AddExamFault(10103);
        }
        MA_EnterMap(prime.examing_area.idx, MAP_TYPE_UPHILL, 0);
    }
}

// 车轮是否压边线
static bool CrashRedLine(prime_t &prime)
{
    bool ret = false;

    int red_lines[][2] = {{0, 1}, {1, 4}, {4, 5}, {5, 8}, {8, 9}};


    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]]);

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        Line red_line;
        MAKE_LINE(red_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]],
                  std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);

        if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
            break;
        }
    }

    return ret;
}

static double DistanceOfHead2Stopline(prime_t &prime)
{
    double dis = 0.0;

    Line upper_edge, lower_edge;

    MAKE_LINE(upper_edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[5],
              std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[6]);
    MAKE_LINE(lower_edge, std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[4],
              std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[3]);

    if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) {
        dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge);
    } else if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge) == REL_POS_LEFT) {
        dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge);
    }

    DEBUG("DistanceOfHead2Stopline dis %f", dis);

    return dis;
}

static double DistanceOfTire2Edge(prime_t &prime)
{
    Line edge;

    MAKE_LINE(edge, 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[8]);

    double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);

    double l2 = DistanceOf(prime.pModeling->points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], edge);

    return MAX(l1, l2);     // 取最远的
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(prime_t &prime)
{
    // 全车都需不在地图中
    bool ret = false;

    /*Polygon carBody, map2;
    PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
    PointF vPoint2;

    if (map->num == 10) {
        vPoint2 = Calc3Point(map->point[8], map->point[9], DistanceOf(map->point[8], map->point[7]), 'L');
    }

    map2.num = 4;
    map2.point = (PointF *)malloc(map2.num * sizeof(PointF));

    if (map->num == 9)
        MakePolygon(&map2, {vPoint, map->point[0], map->point[8], map->point[7]});
    else
        MakePolygon(&map2, {vPoint, map->point[0], map->point[9], vPoint2});

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

    free(carBody.point);
    free(map2.point);*/
    return ret;
}