/*
 * @Description:
 * @Version: 1.0
 * @Autor: Sun Yalun
 * @Date: 2024-10-17 10:19:38
 * @LastEditors: Sun Yalun
 * @LastEditTime: 2024-11-08 15:42:41
 */
#include <iostream>
#include <sstream>
#include "engine.h"
#include <string>
#include <cmath>
#include <iomanip>
#include <fstream>
#include "utils.h"
#include <map>
#include <algorithm>
#include <vector>
std::vector<InterInfo> getInters(const std::string &intercsvPath, const Point &point, double distance)
{
    std::vector<InterInfo> result;
    std::vector<InterInfo> intersections = readIntersectionsFromCSV(intercsvPath);
    if (intersections.empty())
    {
        std::cerr << "没有读取到路口信息。" << std::endl;
        return result;
    }
    // 创建一个距离和路口对象的映射
    std::map<double, InterInfo> dist_to_inter;
    for (const auto &intersection : intersections)
    {
        Point centeroid(0.0, 0.0, 0.0);
        centeroid = calculateCentroid(intersection.inter_coords);
        // 计算中心点到查询点的距离
        double dist = calculateDistance(point.pos_x, point.pos_y, centeroid.pos_x, centeroid.pos_y);
        std::cout << std::fixed << std::setprecision(8);
        // std::cout << "dist: " << dist << std::endl;
        // 如果满足距离要求，把满足条件的路口加入结果
        if (dist <= distance)
        {
            // result.push_back(intersection);
            dist_to_inter[dist] = intersection;
        }
    }
    // 从排序后的列表中提取出路口信息
    for (const auto &dist_inter : dist_to_inter)
    {
        result.push_back(dist_inter.second);
    }
    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的路口。" << std::endl;
        InterInfo inter;
        inter.inter_id = 10000;
        inter.inter_ref_link = {10001};
        inter.inter_type = 10002;
        inter.inter_in_shape = 10003;
        inter.inter_coords = {Point(1.0, 1.0, 0.0)};
        result.push_back(inter);
    }
    return result;
}

std::vector<CrosswalkInfo> getCrosswalks(const std::string &crosscsvPath, const Point &point, double distance)
{
    std::vector<CrosswalkInfo> result;
    std::vector<CrosswalkInfo> crosswalks = readCrosswalksFromCSV(crosscsvPath);
    if (crosswalks.empty())
    {
        std::cerr << "没有读取到人行横道信息。" << std::endl;
        return result;
    }
    std::map<double, CrosswalkInfo> dist_to_cross;
    for (const auto &crosswalk : crosswalks)
    {
        Point centeroid(0.0, 0.0, 0.0);
        centeroid = calculateCentroid(crosswalk.cross_coords);
        // 计算中心点到查询点的距离
        double dist = calculateDistance(point.pos_x, point.pos_y, centeroid.pos_x, centeroid.pos_y);
        std::cout << std::fixed << std::setprecision(8);
        // std::cout << "dist: " << dist << std::endl;
        // 如果满足距离要求，把满足条件的路口加入结果
        if (dist <= distance)
        {
            // result.push_back(crosswalk);
            dist_to_cross[dist] = crosswalk;
        }
    }
    for (const auto &dist_cross : dist_to_cross)
    {
        result.push_back(dist_cross.second);
    }
    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的人行横道。" << std::endl;
        CrosswalkInfo cross;
        cross.cross_id = 20000;
        cross.cross_coords = {Point(2.0, 2.0, 0.0)};
        result.push_back(cross);
    }
    return result;
}

std::vector<TrafficlightInfo> getForwardTrafficlight(const std::string &lightcsvPath, const Point &point, double distance)
{
    std::vector<TrafficlightInfo> result;
    std::vector<TrafficlightInfo> trafficlights = readTrafficlightsFromCSV(lightcsvPath);
    std::map<double, TrafficlightInfo> dist_to_light;
    for (const auto &light : trafficlights)
    {
        Point centeroid(0.0, 0.0, 0.0);
        centeroid = calculateCentroid(light.trafficlight_coords);
        // 计算中心点到查询点的距离
        double dist = calculateDistance(point.pos_x, point.pos_y, centeroid.pos_x, centeroid.pos_y);
        // 如果距离超过指定范围，跳过
        if (dist > distance)
        {
            continue;
        }
        // 计算信号灯中心点到信号灯的方向向量
        double dx = centeroid.pos_x - point.pos_x;
        double dy = centeroid.pos_y - point.pos_y;

        double heading_rad = point.heading;
        double hx = std::cos(heading_rad);
        double hy = std::sin(heading_rad);

        double dot_product = dx * hx + dy * hy;

        double mag_d = std::sqrt(dx * dx + dy * dy);
        double mag_h = std::sqrt(hx * hx + hy * hy);

        double cos_theta = dot_product / (mag_d * mag_h);
        double theta = std::acos(cos_theta);

        double angle_threshold = M_PI / 2;

        // 如果夹角小于阈值，认为信号灯在前方
        if (theta < angle_threshold)
        {
            // result.push_back(light);
            dist_to_light[dist] = light;
        }
    }
    for (const auto &dist_light : dist_to_light)
    {
        result.push_back(dist_light.second);
    }
    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的交通信号灯。" << std::endl;
        TrafficlightInfo tflight;
        tflight.trafficlight_id = 30000;
        tflight.trafficlight_ref_link = 30001;
        tflight.trafficlight_ref_lane = {30002};
        tflight.trafficlight_type = 30003;
        tflight.trafficlight_head = {30004};
        tflight.trafficlight_coords = {Point(3.0, 3.0, 0.0)};
        result.push_back(tflight);
    }
    return result;
}

// 根据位置获取所在道路信息
std::vector<RoadInfo> getLink(const std::string &linkcsvPath, const Point &point)
{
    std::vector<RoadInfo> result;
    std::vector<RoadInfo> roads = readRoadsFromCSV(linkcsvPath);
    if (roads.empty())
    {
        std::cerr << "没有读取到道路信息。" << std::endl;
        return result;
    }
    for (const auto &road : roads)
    {
        bool isNear = false;
        const auto &coords = road.link_coords;
        double min_dist = road.link_lanenum * 3.5 * 0.8; // 根据车道数计算最小距离
        double dist1 = computeBazierDistanceToCurve(point, coords);
        if (dist1 <= min_dist)
        {
            result.push_back(road);
            isNear = true;
            break;
        }
        if (isNear)
        {
            continue;
        }
    }

    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的道路。" << std::endl;
        RoadInfo rInfo;
        rInfo.link_id = 40000;
        rInfo.link_fc = 40001;
        rInfo.link_type = 40002;
        rInfo.link_speed_max = 180;
        rInfo.link_speed_min = 0;
        rInfo.link_direction = 40003;
        rInfo.link_lanenum = 40004;
        rInfo.link_coords = {Point(4.0, 4.0, 0.0)};
        result.push_back(rInfo);
    }
    return result;
}

// 根据所在位置获取所在车道信息
std::vector<LaneInfo> getLane(const std::string &lanecsvPath, const Point &point)
{
    // std::cout << point.pos_x << " " << point.pos_y << " " << point.heading << std::endl;
    std::vector<LaneInfo> result;
    std::vector<LaneInfo> lanes = readLanesFromCSV(lanecsvPath);
    if (lanes.empty())
    {
        std::cerr << "没有读取到车道信息。" << std::endl;
        return result;
    }
    for (const auto &lane : lanes)
    {
        bool isNear = false;
        const auto &coords = lane.lane_coords;
        double min_dist = lane.lane_width;
        double dist1 = computeMinDistanceToCurve(point, coords, 10);
        if (dist1 <= min_dist)
        {
            result.push_back(lane);
            isNear = true;
            break;
        }
        if (isNear)
        {
            continue;
        }
    }
    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的车道。" << std::endl;
        LaneInfo lInfo;
        lInfo.lane_id = 50000;
        lInfo.lane_length = 50001.0;
        lInfo.lane_type = 50002;
        lInfo.lane_width = 50003.0;
        lInfo.lane_turn = 50004;
        lInfo.lane_ref_link = 50005;
        lInfo.lane_speed_max = 180;
        lInfo.lane_speed_min = 0;
        lInfo.lane_coords = {Point(5.0, 5.0, 0.0)};
        result.push_back(lInfo);
    }
    return result;
}
// 根据位置获取指定范围内交通标识牌信息
std::vector<SignalInfo> getSignals(const std::string &signcsvPath, const Point &point, double distance)
{
    std::vector<SignalInfo> result;
    std::vector<SignalInfo> signals = readSignalsFromCSV(signcsvPath);
    if (signals.empty())
    {
        std::cerr << "没有读取到交通标牌信息。" << std::endl;
        return result;
    }
    std::map<double, SignalInfo> dist_to_sign;
    for (const auto &sign : signals)
    {
        Point centeroid(0.0, 0.0, 0.0);
        centeroid = calculateCentroid(sign.sign_coords);
        // 计算中心点到查询点的距离
        double dist = calculateDistance(point.pos_x, point.pos_y, centeroid.pos_x, centeroid.pos_y);
        std::cout << std::fixed << std::setprecision(8);
        // std::cout << "dist: " << dist << std::endl;
        // 如果满足距离要求，把满足条件的路口加入结果
        if (dist <= distance)
        {
            // result.push_back(sign);
            dist_to_sign[dist] = sign;
        }
    }
    for (const auto &dist_sign : dist_to_sign)
    {
        result.push_back(dist_sign.second);
    }
    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的交通标牌。" << std::endl;
        SignalInfo signal;
        signal.sign_id = 60000;
        signal.sign_type = 60001;
        signal.sign_type1 = 60002;
        signal.sign_ref_link = 60003;
        signal.sign_coords = {Point(6.0, 6.0, 0.0)};
    }
    return result;
}
// 根据位置获取指定范围内箭头信息
std::vector<ArrowInfo> getArrow(const std::string &arrowcsvPath, const Point &point, double distance)
{
    std::vector<ArrowInfo> result;
    std::vector<ArrowInfo> arrows = readArrowsFromCSV(arrowcsvPath);
    if (arrows.empty())
    {
        std::cerr << "没有读取到箭头信息。" << std::endl;
        return result;
    }
    std::map<double, ArrowInfo> dist_to_arrow;
    for (const auto &arrow : arrows)
    {
        Point centeroid(0.0, 0.0, 0.0);
        centeroid = calculateCentroid(arrow.arrow_coords);
        // 计算中心点到查询点的距离
        double dist = calculateDistance(point.pos_x, point.pos_y, centeroid.pos_x, centeroid.pos_y);
        std::cout << std::fixed << std::setprecision(8);
        // std::cout << "dist: " << dist << std::endl;
        if (dist > distance)
        {
            continue;
        }
        double dx = centeroid.pos_x - point.pos_x;
        double dy = centeroid.pos_y - point.pos_y;
        double heading_rad = point.heading;
        double hx = std::cos(heading_rad);
        double hy = std::sin(heading_rad);
        double dot_product = dx * hx + dy * hy;
        double mag_d = std::sqrt(dx * dx + dy * dy);
        double mag_h = std::sqrt(hx * hx + hy * hy);
        double cos_theta = dot_product / (mag_d * mag_h);
        double theta = std::acos(cos_theta);
        double angle_threshold = M_PI / 2;
        if (theta < angle_threshold)
        {
            // result.push_back(arrow);
            dist_to_arrow[dist] = arrow;
        }
    }
    for (const auto &dist_arrow : dist_to_arrow)
    {
        result.push_back(dist_arrow.second);
    }
    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的箭头。" << std::endl;
        ArrowInfo aInfo;
        aInfo.arrow_id = 70000;
        aInfo.arrow_ref_lane = 70001;
        aInfo.arrow_direction = {70002};
        aInfo.arrow_coords = {Point(7.0, 7.0, 0.0)};
        result.push_back(aInfo);
    }
    return result;
}

// 根据位置获取指定范围内车道停止线信息
std::vector<StoplineInfo> getStopline(const std::string &stoplinecsvPath, const Point &point, double distance)
{
    std::vector<StoplineInfo> result;
    std::vector<StoplineInfo> stoplines = readStopLinesFromCSV(stoplinecsvPath);
    if (stoplines.empty())
    {
        std::cerr << "没有读取到车道停止线信息。" << std::endl;
    }
    std::map<double, StoplineInfo> dist_to_stopline;
    for (const auto &stopline : stoplines)
    {
        Point centeroid(0.0, 0.0, 0.0);
        centeroid = calculateCentroid(stopline.stopline_coords);
        // 计算中心点到查询点的距离
        double dist = calculateDistance(point.pos_x, point.pos_y, centeroid.pos_x, centeroid.pos_y);
        std::cout << std::fixed << std::setprecision(8);
        // std::cout << "dist: " << dist << std::endl;
        if (dist > distance)
        {
            continue;
        }
        double dx = centeroid.pos_x - point.pos_x;
        double dy = centeroid.pos_y - point.pos_y;
        double heading_rad = point.heading;
        double hx = std::cos(heading_rad);
        double hy = std::sin(heading_rad);
        double dot_product = dx * hx + dy * hy;
        double mag_d = std::sqrt(dx * dx + dy * dy);
        double mag_h = std::sqrt(hx * hx + hy * hy);
        double cos_theta = dot_product / (mag_d * mag_h);
        double theta = std::acos(cos_theta);
        double angle_threshold = M_PI / 2;
        if (theta < angle_threshold)
        {
            // result.push_back(stopline);
            dist_to_stopline[dist] = stopline;
        }
    }
    for (const auto &dist_stopline : dist_to_stopline)
    {
        result.push_back(dist_stopline.second);
    }
    if (result.size() == 0)
    {
        // std::cerr << "没有找到符合条件的车道停止线。" << std::endl;
        StoplineInfo sInfo;
        sInfo.stopline_id = 80000;
        sInfo.stopline_ref_link = 80001;
        sInfo.stopline_ref_lane = {80002};
        sInfo.stopline_color = 80003;
        sInfo.stopline_type = 80004;
        sInfo.stopline_coords = {Point(8.0, 8.0, 0.0)};
        result.push_back(sInfo);
    }
    return result;
}