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

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

#include "stop_and_start.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"

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

using namespace std;


const uint32_t STOP_CAR_TIME = D_SEC(1);

const double EPSILON = 1e-3;

static bool testing = false;

static PointF stopPoint;

static int mapIndex = 0;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool stopCar = false;
static uint32_t stopCarTime;
static bool occurCrashRedLine = false;
static bool slideLongDistance = false;
static bool slideNormalDistance = false;
static bool reportSlideFault = false;
static bool reportStartTimeout = false;
static bool handBreakActive = false;
static bool CrashRedLine(const Polygon *map, const car_model *car);
static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car);
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);

void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("进入坡起项目");
    testing = true;
    mapIndex = index;
    prevMoveDirect = moveDirect;

    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }

    occurCrashRedLine = false;
    stopCar = false;
    slideLongDistance = false;
    slideNormalDistance = false;
    reportSlideFault = false;
    reportStartTimeout = false;
    handBreakActive = false;

    MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
}

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    if (CrashRedLine(map, car)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
            AddExamFault(13, rtkTime);
            DEBUG("车轮压线");
        }
        occurCrashRedLine = true;
    } else {
        occurCrashRedLine = false;
    }

    if (ExitTestArea(map, car)) {
        // 驶离测试区
        testing = false;
    }

    // 距离检测
    {
        vector<double> dtox;
        vector<Line> line_set;
        Line distance_line;

        MakeLine(&distance_line, &map->point[0], &map->point[8]);
        line_set.push_back(distance_line);
        DistanceOfTire2X(dtox, car, line_set);
        MA_SendDistance(dtox[0], dtox[1]);
    }

    if (prevMoveDirect != moveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        } else if (stopCar && !handBreakActive) {
            // 检查是否拉住手刹
            handBreakActive = true;
            AddExamFault(19, rtkTime);
            DEBUG("没拉手刹");
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !stopCar) {
            // 这里判断停车状态
            stopCar = true;
            stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

            stopPoint = car->carXY[car->body[0]];

            double dis1 = DistanceOfHead2Stopline(map, car);
            double dis2 = DistanceOfTire2Edge(map, car);

            DEBUG("DIS1 = %f  DIS2 = %f", dis1, dis2);

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

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

        if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
            handBreakActive = true;
        }
    }

    // 判断起步后滑状态
    if (stopCar) {
        if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
            // 发生后滑
            double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);

            if (slideDistance > examParam.ramp_slide_yellow_distance) {
                slideNormalDistance = true;
            }

            if (slideDistance > examParam.ramp_slide_red_distance && !slideLongDistance && !reportSlideFault) {
                // 后滑超过30厘米, 不合格
                AddExamFault(16, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
                reportSlideFault = true;
            }
        }


        if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
                DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
            if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > examParam.ramp_start_car_limit_time) {
                // 起步时间超过30秒，不合格
                AddExamFault(15, rtkTime);
                DEBUG("起步时间超过30秒");
                reportStartTimeout = true;
            }
        }

        if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) {
            // 车尾驶过停止杆
            if (slideNormalDistance && !slideLongDistance && !reportSlideFault) {
                reportSlideFault = true;
                // 后滑超过10厘米，但没超过30厘米
                AddExamFault(20, rtkTime);
                DEBUG("后滑超过10厘米，但没超过30厘米");
            }
        }
    }

    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
    }

    return testing ? 1 : 0;
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const car_model *car)
{
    bool ret = false;

    Line red_line;
    const int red_lines[][2] = {{0, 8}};

    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    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, frontAxle) == GM_Intersection ||
                IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
            break;
        }
    }

    return ret;
}

static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
{
    double dis = 0.0;

    int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
    int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);

    DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
    DEBUG("rel1 %d rel2 %d", rel1, rel2);

    if (rel1 == 1) {
        Line line1;

        MakeLine(&line1, &map->point[4], &map->point[3]);

        dis = DistanceOf(car->carXY[car->body[0]], line1);
    } else if (rel2 == -1) {
        Line line2;

        MakeLine(&line2, &map->point[5], &map->point[6]);

        dis = DistanceOf(car->carXY[car->body[0]], line2);
    }

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

    return dis;
}

static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
{
    Line edge;

    MakeLine(&edge,  &map->point[0], &map->point[8]);

    double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);

    double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);

    return (l1+l2)/2.0;
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model *car)
{
    // 全车都需不在地图中
    bool ret = false;

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

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

    MakePolygon(&map2, {vPoint, map->point[0], map->point[7], map->point[8]});

    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;
}