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

#include "drive_straight.h"
#include "../driver_test.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "road_exam.h"
#include <cmath>

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

static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;

void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) {
    if (index == -1)
        return;

    DEBUG("进入路考直线行驶地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);

    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS");
        PlayTTS(RoadMapList[index].tts.c_str());
    } else {
        DEBUG("没有TTS");
    }

    crossStartLine = false;
    reportOffsetOver = false;
}

int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    Line road_edge;

    static PointF startPoint;

    MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);

    if (!crossStartLine) {
        PointF p1, p2;

        p1.X = RoadMapList[index].stopLine.X1;
        p1.Y = RoadMapList[index].stopLine.Y1;
        p2.X = RoadMapList[index].stopLine.X2;
        p2.Y = RoadMapList[index].stopLine.Y2;

        if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) {
            crossStartLine = true;
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
        }
    } else {
        double runDistance;
        runDistance = DistanceOf(car->basePoint, startPoint);

        if (runDistance < 100.0) {
            if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > 0.3) {
                DEBUG("直线偏移大于30厘米");
                // 偏移大于30厘米，不合格
                AddExamFault(30, rtkTime);
                reportOffsetOver = true;
            }
        } else {
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
            reportOffsetOver = false;
        }
    }

    if (ExitSonArea(index, RoadMapList, car)) {
        DEBUG("离开直线行驶区域");
        return -1;
    }

    return index;
}