//
// Created by YY on 2019/11/4.
//

#include "driving_curve.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"

#include <vector>
#include <cstdlib>

using namespace std;

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

enum {
    DRIVING_ON_CURVE
};

const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool testing = false;
static uint32_t stopTimepoint = 0;

static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
static struct calc_zone_t {
    int leftStart;
    int leftEnd;
    int rightStart;
    int rightEnd;
} calcZone;

static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car);

void StartDrivingCurve(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);
    }
    reportStopCarTimeout = false;
    crashRedLine = false;

    calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    Line start, end;
    Line axial;

    MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);

//    DEBUG("START 线 %d -- %d", calcZone.leftStart, calcZone.rightStart);
//    DEBUG("END 线 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);

    MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);

    if (IntersectionOf(start, axial) == GM_None) {
        // 向起点查找
        int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart;

        while (calcZone.leftStart > 0 || calcZone.rightStart > 0) {
            if (calcZone.leftStart > 0)
                tempLeft = calcZone.leftStart - 1;
            if (calcZone.rightStart > 0)
                tempRight = calcZone.rightStart - 1;
//            DEBUG("START 向起点查找 %d -- %d", tempLeft, tempRight);
            MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]);

            if (IntersectionOf(start, axial) == GM_Intersection) {
                // 保持之前的线
                break;
            } else {
                calcZone.leftStart = tempLeft;
                calcZone.rightStart = tempRight;

                if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) {
                    // 车辆丢失，重新搜索
                    calcZone.leftStart = calcZone.rightStart = 0;
                    calcZone.leftEnd = calcZone.rightEnd = 0;

                    DEBUG("车辆丢失，重新搜索");

                    while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
                        MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);

                        if (IntersectionOf(start, axial) == GM_Intersection) {
                            while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
                                MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
                                if (IntersectionOf(start, axial) == GM_None) {
                                    break;
                                }
                                if (calcZone.leftStart < map->num)
                                    calcZone.leftStart++;
                                if (calcZone.rightStart < map2->num)
                                    calcZone.rightStart++;
                            }
                            break;
                        }

                        calcZone.leftEnd = calcZone.leftStart;
                        calcZone.rightEnd = calcZone.rightStart;

                        if (calcZone.leftStart < map->num)
                            calcZone.leftStart++;
                        if (calcZone.rightStart < map2->num)
                            calcZone.rightStart++;
                    }

                    if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
                        // 离开场地
                        DEBUG("离开曲线场地");
                        testing = false;
                        goto TEST_END;
                    }

                    break;
                }
            }
        }
    } else {
        // 向终点查找
        do {
            if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
                break;
            }
            if (calcZone.leftStart < map->num)
                calcZone.leftStart++;
            if (calcZone.rightStart < map2->num)
                calcZone.rightStart++;
//            DEBUG("START 向终点查找 %d -- %d", calcZone.leftStart, calcZone.rightStart);
            MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
        } while (IntersectionOf(start, axial) == GM_Intersection);
    }

    MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);

    if (IntersectionOf(end, axial) == GM_None) {
        // 向终点查找
        int tempLeft = calcZone.leftEnd, tempRight = calcZone.rightEnd;
        while (calcZone.leftEnd < map->num || calcZone.rightEnd < map2->num) {
            if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
                break;
            }
            if (calcZone.leftEnd < map->num)
                tempLeft = calcZone.leftEnd + 1;
            if (calcZone.rightEnd < map2->num)
                tempRight = calcZone.rightEnd + 1;
//            DEBUG("END 向终点查找 %d -- %d", tempLeft, tempRight);
            MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);

            if (IntersectionOf(end, axial) == GM_Intersection) {
                // 保持之前的线
                break;
            } else {
                calcZone.leftEnd = tempLeft;
                calcZone.rightEnd = tempRight;
            }
        }
    } else {
        // 向起点查找
        do {
            if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
                break;
            }
            if (calcZone.leftEnd > 0)
                calcZone.leftEnd--;
            if (calcZone.rightEnd > 0)
                calcZone.rightEnd--;
//            DEBUG("END 向起点查找 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
            MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
        } while (IntersectionOf(end, axial) == GM_Intersection);
    }

    DEBUG("calcZone leftStart %d leftEnd %d rightStart %d rightEnd %d", calcZone.leftStart, calcZone.leftEnd, calcZone.rightStart, calcZone.rightEnd);

    if (CrashRedLine(map, map2, car, &calcZone)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 车轮压边线，不合格
            AddExamFault(27, rtkTime);
            DEBUG("车轮压边线");
        }
    } else {
        crashRedLine = false;
    }

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

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {

        }
        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 && !reportStopCarTimeout) {
            // 停车超2秒，不合格
            AddExamFault(28, rtkTime);
            DEBUG("中途停车");
            reportStopCarTimeout = true;
        }
    }

TEST_END:
    return testing ? 1 : 0;
}

static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;
    Polygon bigMap;

    bigMap.num = map->num + map2->num;
    bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));

    int i = 0;
    for (; i < map->num; ++i) {
        bigMap.point[i] = map->point[i];
    }
    for (int j = map2->num; j > 0; --j) {
        bigMap.point[i++] = map2->point[j-1];
    }

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

    free(carBody.point);
    free(bigMap.point);

    return ret;
}

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

    Line frontTireAxial, rearTireAxial;
    Line redLine;

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

    int s = zone->leftStart;
    for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
        MakeLine(&redLine, &map->point[s], &map->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            return true;
        }
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            return true;
        }
        s = e;
    }

    s = zone->rightStart;
    for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
        MakeLine(&redLine, &map2->point[s], &map2->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            return true;
        }
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            return true;
        }
        s = e;
    }

    return ret;
}