//
// 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 "../master/comm_if.h"
#include "../common/apptimer.h"
#include <cmath>

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

static const double CHECK_STAGE_DISTANCE = 100.0;
static const double MAX_OFFSET_DISTANCE = 0.3;

static int ttsPlayEnd;
static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;
static double distanceToStartSum;
static int examTtsSeq;

static double CalcDistance2Edge(road_t &road,  const car_model *car);
static void PlayTTSTimeout(union sigval sig);

void StartDriveStraightExam(std::string tts) {
    DEBUG("开始直线行驶");

    ttsPlayEnd = 0;
    if (!tts.empty()) {
        examTtsSeq = PlayTTS(tts.c_str(), NULL);
    } else {
        examTtsSeq = PlayTTS("请保持直线行驶", NULL);
    }

    distanceToStartSum = 0;
    reportOffsetOver = false;

    AppTimer_delete(PlayTTSTimeout);
    AppTimer_add(PlayTTSTimeout, D_SEC(5));
}

int ExecuteDriveStraightExam(road_t &road,  const car_model *car,
                             const trigger_line_t *item, const struct RtkTime *rtkTime) {
    static PointF startPoint;

    double dis2roadEdge = 0;

    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
        startPoint = car->basePoint;
        edgeDistance = CalcDistance2Edge(road, car);           // 基准边距

        DEBUG("当前基准路边间距 %f", edgeDistance);
    }

    if (ttsPlayEnd != 2)
        return 1;

    double distanceToStart = DistanceOf(car->basePoint, startPoint);
    dis2roadEdge = CalcDistance2Edge(road, car);

    DEBUG("路边间距 %f --- %f", dis2roadEdge, edgeDistance);

    if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) {
        DEBUG("直线偏移大于30厘米");
        // 偏移大于30厘米，不合格
        AddExamFault(30, rtkTime);
        reportOffsetOver = true;

        //////////////////////////////////////////////
//        startPoint = car->basePoint;
//        edgeDistance = dis2roadEdge;
//        reportOffsetOver = false;
    }

    if (distanceToStart > CHECK_STAGE_DISTANCE) {
        DEBUG("复位边距偏移量");
        startPoint = car->basePoint;
        edgeDistance = dis2roadEdge;
        reportOffsetOver = false;
        distanceToStartSum += distanceToStart;
        distanceToStart = 0;
    }

    if (distanceToStart + distanceToStartSum > 105) {
        DEBUG("离开直线行驶区域");
        PlayTTS("直线行驶结束", NULL);
        return -1;
    }
    return 1;
}

void DriveStraightTTSDone(int id)
{
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        DEBUG("StopCarTTSDone %d", id);
        ttsPlayEnd = 1;
        AppTimer_delete(PlayTTSTimeout);
    }
}

void TerminateDriveStraightExam(void)
{
    AppTimer_delete(PlayTTSTimeout);
}

static void PlayTTSTimeout(union sigval sig)
{
    AppTimer_delete(PlayTTSTimeout);

    ttsPlayEnd = 1;
}

static double CalcDistance2Edge(road_t &road,  const car_model *car)
{
    PointF vp;
    bool get_vp = false;
    double distance = 0;

    // 检测道路边缘线
    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (VerticalPointOnLine(car->basePoint, edge, vp)) {
                get_vp = true;
                goto FIND_VP_END;
            }

            p1 = p2;
        }
    }

FIND_VP_END:
    if (get_vp) {
        DEBUG("得到垂点 %d: %f, %f -- %f, %f", road.id, car->basePoint.X, car->basePoint.Y, vp.X, vp.Y);
        distance = DistanceOf(car->basePoint, vp);
    } else {
        // 没有找到匹配线端，按最小距离顶点计算
        DEBUG("无垂点");
        distance = 100;
        for (int i = 0; i < road.leftEdge.size(); ++i) {
            for (int j = 0; j < road.leftEdge[i].points.size(); ++j) {
                double x;
                if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) {
                    distance = x;
                }
            }
        }
    }

    return distance;
}