//
// 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"

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

using namespace std;

const double STOP_DISTANCE_THRESHOLD_RED = 0.5;
const double EDGE_DISTANCE_THRESHOLD_RED = 0.5;
const double EDGE_DISTANCE_THRESHOLD_YELLOW = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const uint32_t CAR_START_TIMEOUT = D_SEC(30);
const uint32_t STOP_CAR_TIME = D_SEC(2);

const double EPSILON = 1e-3;

static bool testing = false;

static PointF stopPoint;

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 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 moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("进入坡起项目");
    testing = true;

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

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

    if (prevMoveDirect != moveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        }
        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 > STOP_DISTANCE_THRESHOLD_RED) {
                // 距离停止线前后超出50厘米
                AddExamFault(12, rtkTime);
                DEBUG("距离停止线前后超出50厘米，不合格");
            } else if (fabs(dis1) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(17, rtkTime);
                DEBUG("前保险没有位于停止带内，但没有超出50厘米");
            }

            if (dis2 > EDGE_DISTANCE_THRESHOLD_RED) {
                // 距离边线超出50厘米,不合格
                AddExamFault(14, rtkTime);
                DEBUG("距离边线超出50厘米");
            } else if (dis2 > EDGE_DISTANCE_THRESHOLD_YELLOW) {
                // 距离边线超出30厘米，不合格
                AddExamFault(18, rtkTime);
                DEBUG("距离边线超出30厘米");
            }

            // 检查是否拉住手刹
            if (ReadCarStatus(HAND_BREAK) != BREAK_ACTIVE) {
                AddExamFault(19, rtkTime);
                DEBUG("没拉手刹");
            }
        }
    }

    // 判断起步后滑状态
    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 > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            }

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !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 > CAR_START_TIMEOUT) {
                // 起步时间超过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厘米");
            }
        }
    }

    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)
{
    // 在8->7线的右侧
    for (int i = 0; i < car->bodyNum; ++i) {
        if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
            return false;
    }
    return true;
}