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

#include <map>
#include <algorithm>

#include "through_something.h"
#include "../driver_test.h"
#include "../test_common/Geometry.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../test_common/car_sensor.h"
#include "road_exam.h"
#include "../utils/xconvert.h"

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

using namespace std;

static map <int, int> TargetReduceRec;
static map <int, int> TargetReduceRec2;

#define NOT_ENTER           1
#define ENTER_Z             2
#define REDUCE_SPEED        4
#define STOP_CAR            8
#define OVER_SPEED          16

static void SetTargetReduceRec(map<int, int> &table, int key, int status)
{
    auto it = table.find(key);

    if (it != table.end()) {
        table.erase(it);
    }
    table.insert(pair<int, int>(key, status));
}

static int GetTargetReduceRec(map<int, int> &table, int key)
{
    auto it = table.find(key);

    if (it != table.end()) {
        return it->second;
    }
    return NOT_ENTER;
}

static void RemoveTargetReduceRec(map<int, int> &table, int key)
{
    auto it = table.find(key);

    if (it != table.end()) {
        table.erase(it);
    }
}

void ResetTarget(road_exam_map &RoadMap)
{
    TargetReduceRec.clear();
    TargetReduceRec2.clear();
}

double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    vector<double> nearbyTarget;        // 学校、公交站、人行道区域的距离

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return 100000;
    // 路口
    for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
        PointF point;
        double distance;

        point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1;
        point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1;

        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);

        int key = roadIndex * 100 + i;
        int rec = GetTargetReduceRec(TargetReduceRec, key);

        if (distance > 1e-3 && distance < examParam.crossing_break_valid_distance) {
            if (rec == NOT_ENTER) {
                SetTargetReduceRec(TargetReduceRec, key, ENTER_Z);
            }
            // 记录刹车
            if (dobreak && !(rec & REDUCE_SPEED)) {
                DEBUG("检测到路口刹车动作");
                SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED);
            }
        } else if (distance > 1e-3 && distance < examParam.crossing_stop_valid_distance) {
            // 路口停车观察
            if (moveDirect == 0 && !(rec & STOP_CAR)) {
                DEBUG("检测到路口停车动作");
                SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR);
            }
        } else if (distance > examParam.crossing_break_valid_distance + 5 && rec != NOT_ENTER) {
            RemoveTargetReduceRec(TargetReduceRec, key);
        }
    }

    if (RoadMap.calibrate == 1)
        goto GET_DISTANCE_OF_NEARBY_TARGET;

    // 人行道、公交站、学校
    for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
        if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
            continue;

        if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
            double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
            double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);

            int key =  i;
            int rec = GetTargetReduceRec(TargetReduceRec2, key);

            if (distance1 < -1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(0);
            } else if (distance1 > 1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(distance1);
            }

            if (distance1 > 1e-3 && distance1 < examParam.crossing_break_valid_distance) {
                if (rec == NOT_ENTER) {
                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                }
                // 记录刹车，停车
                if (dobreak && !(rec & REDUCE_SPEED)) {
                    DEBUG("检测到人行道等刹车动作");
                    SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
                }
            } else if (distance1 > examParam.crossing_break_valid_distance + 5 && rec != NOT_ENTER) {
                RemoveTargetReduceRec(TargetReduceRec2, key);
            }
        } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) {
            double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
            double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);

            int key =  i;
            int rec = GetTargetReduceRec(TargetReduceRec2, key);

            if (distance1 < -1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(0);
                if (rec == NOT_ENTER) {
                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                }
                if (ConvertMs2KMh(speed) > examParam.cross_school_max_speed && !(rec & OVER_SPEED)) {
                    SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED);

                    DEBUG("通过学校区域超速 %f kmh", ConvertMs2KMh(speed));
                    AddExamFault(41101, rtkTime);
                }
            } else if (distance1 < -1e-3 && distance2 < -1e-3) {
                if (rec != NOT_ENTER) {
                    RemoveTargetReduceRec(TargetReduceRec2, key);
                }
            } else {
                nearbyTarget.push_back(distance1);
                if (rec != NOT_ENTER) {
                    RemoveTargetReduceRec(TargetReduceRec2, key);
                }
            }
        }
    }

    GET_DISTANCE_OF_NEARBY_TARGET:
    if (nearbyTarget.size() > 0) {
        sort(nearbyTarget.begin(), nearbyTarget.end());
        return nearbyTarget[0];
    }
    return 100000;
}

void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
{
    RECHECK:
    for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) {
            int road = it->first / 100;
            int x = it->first % 100;

            if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
                DEBUG("越过路口 road %d 路口 %d", road, x);

                CrossRoadCallback(RoadMap, road, x, ExamSchemeCrossing(RoadMap, road, x), car);

                if (RoadMap.calibrate == 0) {
                    if (!(it->second & REDUCE_SPEED)) {
                        // 不按规定减速，不合格
                        DEBUG("不按规定减速");

                        if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
                            AddExamFault(40701, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_LEFT) {
                            AddExamFault(40801, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_RIGHT) {
                            AddExamFault(40901, rtkTime);
                        }
                    }
                    if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
                        // 不停车瞭望，不合格
                        DEBUG("不停车瞭望");
                        if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
                            AddExamFault(40701, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_LEFT) {
                            AddExamFault(40801, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_RIGHT) {
                            AddExamFault(40901, rtkTime);
                        }
                    }
                }
                RemoveTargetReduceRec(TargetReduceRec, it->first);
                goto RECHECK;
            }
    }

    if (RoadMap.calibrate == 1)
        return;

    RECHECK2:
    for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
        int x = it->first;
        Line line;
        int roadIndex = 0;

        if (RoadMap.specialAreas[x].type == SCHOOL_AREA) {          // 离开学校区域不按跨线判定
            continue;
        }

        for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
            if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
                break;
        }

        PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
                                                   RoadMap.specialAreas[x].area[0]);
        MakeLine(&line, &RoadMap.specialAreas[x].area[0], &point2);

        if (CrashTheLine(line, car, CarModelList)) {
            if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
                !(it->second & REDUCE_SPEED)) {
                DEBUG("人行道 不按规定减速");
                AddExamFault(41001, rtkTime);
            }
            if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
                !(it->second & REDUCE_SPEED)) {
                DEBUG("公交站 不按规定减速");
                AddExamFault(41201, rtkTime);
            }
            RemoveTargetReduceRec(TargetReduceRec2, it->first);
            goto RECHECK2;
        }
    }
}