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

#include <cstdlib>
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../driver_test.h"
#include "../jni_log.h"
#include "park_bottom.h"
#include "stop_and_start.h"
#include "park_edge.h"
#include "driving_curve.h"
#include "turn_a90.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
#include "../common/observer.h"

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

ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;

static bool ProximityArea(Line &base_line, Line &line);
static bool CrossingStartLine(Line &trace, Line &start_line);
static void ProximityReminders(prime_t &prime);
static void DetectCarMove(prime_t &prime);

void AreaExam(prime_t &prime)
{
    switch (prime.curr_exam_map.type) {
        case MAP_TYPE_PARK_BUTTOM:
            break;
        case MAP_TYPE_PARK_EDGE:
            break;
        case MAP_TYPE_TURN_90:
            break;
        case MAP_TYPE_UPHILL:
            break;
        case MAP_TYPE_CURVE:
            break;
        default:
            if (EnterMap()) {

            }
            break;
    }
    DetectCarMove(prime);
}

static void DetectCarMove(prime_t &prime)
{
    static move_status_t prevMove = STOP;
    if (prime.pMotion->move != prevMove) {
        // Notify
        CarMoveEvent.Notify(prime.pMotion->move);
        prevMove = prime.pMotion->move;
    }
}

void RegisterCarMoveObserver(void (*ptr)(move_status_t))
{
    CarMoveEvent.Connect(ptr);
}

void UnregisterCarMoveObserver(int handle)
{
    CarMoveEvent.Disconnect(handle);
}

void EnterMap(prime_t &prime)
{
    if (prime.curr_exam_map.type != 0) {
        return;
    }

    if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
        return;
    }

    PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
    PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];

    Line left_trace, right_trace;

    MAKE_LINE(left_trace, lp1, lp2);
    MAKE_LINE(right_trace, rp1, rp2);

    Line start_line;
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
            prime.curr_exam_map.map_idx = i;
            StartParkBottom(prime);
            return;
        }
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
            prime.curr_exam_map.map_idx = i;
            StartParkBottom(prime);
            return;
        }
    }

    for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }

    for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
        PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
        MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_UPHILL;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
    for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_CURVE;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
    for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_TURN_90;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
}

// 车轮驶过线，且车头位于右侧
static bool CrossingStartLine(Line &trace, Line &start_line)
{
    PointF head = {.X = trace.X1, .Y = trace.Y1};

    if (IntersectionOf(trace, start_line) == GM_Intersection
        && IntersectionOfLine(head, start_line) == RELATION_RIGHT) {
        return true;
    }
    return false;
}

void FarawayMap(prime_t &prime)
{
    if (prime.arriving_map.type != 0) {
        PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
        PointF car_head_trend = PointExtend(car_head, 7, prime.pModeling->yaw);
        Line car_head_line;
        MAKE_LINE(car_head_line, car_head, car_head_trend);


    }
}

static void ProximityReminders(prime_t &prime)
{
    if (prime.curr_exam_map.type != MAP_TYPE_NONE && prime.arriving_map.type != MAP_TYPE_NONE) {
        return;
    }

    PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
    PointF car_head_trend = PointExtend(car_head, 6, prime.pModeling->yaw);
    Line car_head_line;
    MAKE_LINE(car_head_line, car_head, car_head_trend);

    Line start_line;
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {  // 左右2条控制线都可作为入口
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);

        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);

    }
    for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
    }

    for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
        PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
        MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
    }
    for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
    }
    for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
    }
}

// 判断是否接近场地的起始线
// 车头趋势线是否和入口线相交
// 和入口线的夹角
static bool ProximityArea(Line &base_line, Line &line)
{
    PointF head = {.X = line.X1, .Y = line.Y1};

    if (IntersectionOf(base_line, line) == GM_Intersection
        && DistanceOf(head, base_line) > 1) {
        double angle = AngleOfTowLine(base_line, line);
        if (angle >= 240 && angle <= 300) {
            return true;
        }
    }

    return false;
}

void TerminateAreaExam(void)
{
    CurrExamMapIndex = -1;
}

void InitAreaExam(void)
{
    CurrExamMapIndex = -1;
    ResetOdo();
}