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

// 4----------------------|5
//                        |
//                        |
// 3----------|2          |
//            |           |
//            |           |
//            |           |
//            |           |
//            |           |
//            |1          |0

#include "right_corner.h"
#include "../test_common/Geometry.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
#include "../utils/xconvert.h"
#include "../defs.h"
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"
#include "area_exam.h"

#include <tuple>
#include <vector>
#include <cstdlib>

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

using namespace std;

static int enterAreaHeading;
static bool turnLeftFinished;
static bool crashRedLine;

static bool CrashRedLine(prime_t &prime);

void StartRightCorner(prime_t &prime)
{
    DEBUG("进入直角转弯场地");

    enterAreaHeading = (int) prime.pModeling->yaw;
    crashRedLine = false;
    turnLeftFinished = false;

    MA_EnterMap(prime.examing_area.idx, MAP_TYPE_RIGHT_CORNER, 1);
}

void StopRightCorner(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_RIGHT_CORNER)
        return;
    DEBUG("退出直角转弯场地");

    prime.examing_area.type = MAP_TYPE_NONE;
}

void TestRightCorner(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_RIGHT_CORNER)
        return;
    int az = (int) prime.pModeling->yaw;
    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;

    if (CrashRedLine(prime)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 碾压道路边缘,不合格
            AddExamFault(20701);
            DEBUG("碾压道路边缘");
        }
    } else {
        crashRedLine = false;
    }

    // 检查转向状态
    if (ABS(az - enterAreaHeading) > 180) {
        az = 360 - ABS(az-enterAreaHeading);
    } else {
        az = ABS(az - enterAreaHeading);
    }

    if (az >= 30) {
        if (!turnLeftFinished) {
            char turn_direct;
            if((( az + 360 - enterAreaHeading) % 360) < 180) {
                DEBUG("右转");
                turn_direct = 'R';
            } else {
                DEBUG("左转");
                turn_direct = 'L';
            }
            // 转向灯未开启，扣10分
            if ((turn_direct == 'R' && prime.sensor.turn_signal_lamp != RIGHT_TURN_LIGHT) ||
                    (turn_direct == 'L' && prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT)) {
                DEBUG("转向灯未开启");
                AddExamFault(20702);
            }
        }
        turnLeftFinished = true;
    }
}

// 车轮是否压边线
static bool CrashRedLine(prime_t &prime)
{
    bool ret = false;

    const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};

    Line frontAxle, rearAxle;

    // 仅看车轮外侧
    MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        Line red_line;
        MAKE_LINE(red_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]],
                  std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);

        if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
            break;
        }
    }

    return ret;
}