//
// 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 {
    int leftStart;
    int leftEnd;
    int rightStart;
    int rightEnd;
} calcZone;

static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *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 = -1;
}

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;
    Polygon zone;

    if (calcZone.leftStart == -1) {
        calcZone.leftStart = 0;
    }
    if (calcZone.rightStart == -1) {
        calcZone.rightStart = 0;
    }

    start.X1 = map->point[calcZone.leftStart].X;
    start.Y1 = map->point[calcZone.leftStart].Y;

    start.X2 = map->point[calcZone.rightStart].X;
    start.Y2 = map->point[calcZone.rightStart].Y;

    zone.num = 4;
    zone.point = (PointF *) malloc(zone.num * sizeof(PointF));
    zone.point[0] = car->carXY[car->left_front_tire[TIRE_OUTSIDE]];
    zone.point[1] = car->carXY[car->right_front_tire[TIRE_OUTSIDE]];
    zone.point[2] = car->carXY[car->right_rear_tire[TIRE_OUTSIDE]];
    zone.point[3] = car->carXY[car->left_rear_tire[TIRE_OUTSIDE]];

    if (IntersectionOf(start, &zone) == GM_None) {

    }

    free(zone.point);

    return testing ? 1 : 0;
}

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

    car_model_cache_t *prev_car = GetCarModelCache(1);
    if (prev_car == NULL)
        return false;

    // 按4个轮子的外侧计算
    Line front_left_tire_track, front_right_tire_track, rear_left_tire_track, rear_right_tire_track;
    MakeLine(&front_left_tire_track,  &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&front_right_tire_track,  &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_left_tire_track,  &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_right_tire_track,  &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);

    Line line;

    line.X1 = left->point[0].X;
    line.Y1 = left->point[0].Y;
    for (int i = 1; i < left->num; ++i) {
        line.X2 = left->point[i].X;
        line.Y2 = left->point[i].Y;

        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
    }

    line.X1 = right->point[0].X;
    line.Y1 = right->point[0].Y;
    for (int i = 1; !ret && i < right->num; ++i) {
        line.X2 = right->point[i].X;
        line.Y2 = right->point[i].Y;

        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
    }

    return ret;
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(left->point[0], right->point[0], car->points[i]) != 1)
            return false;
    }
    return true;
}