/*
 * @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;
    }
    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;
    }
    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;
    }
    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;
    }
    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;
    }
    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;
    }
    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;
    }
    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;
    }
    return result;
}
// 测试代码
int main(int argc, char **argv) {

    std::string mapCsv_path = argv[1];
    std::string interPath = mapCsv_path + "/hd_intersections.csv";
    std::string crossPath = mapCsv_path + "/hd_crosswalks.csv";
    std::string trafficlightPath = mapCsv_path + "/hd_trafficlight.csv";
    std::string roadPath = mapCsv_path + "/hd_link.csv";
    std::string arrowPath = mapCsv_path + "/hd_arrow.csv";
    std::string stoplinePath = mapCsv_path + "/hd_stopline.csv";
    std::string lanePath = mapCsv_path + "/hd_lane.csv";
    std::string signPath = mapCsv_path + "/hd_sign.csv";

    // Point chaxun_point(789043.30, 4151434.91, 2*M_PI / 3.0);
    // Point chaxun_point(789077.20, 4151506.82, -2*M_PI / 3.0);
    // Point chaxun_point(789065.83, 4151328.90, 2*M_PI / 3.0);
    // Point chaxun_point(789084.10, 4151281.66, 2*M_PI / 3.0);
    Point chaxun_point(258384.896, 4149921.26, 2*M_PI / 3.0);
    double distance = 60;

    std::vector<InterInfo> chaxun_inters = getInters(interPath, chaxun_point, distance);
    // 打印查询到的路口信息
    for (const auto& inter : chaxun_inters) {
        std::cout << "路口id: " << inter.inter_id << std::endl;
        std::cout << "路口类型为: " << inter.inter_type << std::endl;
        std::cout << "路口形态为: " << inter.inter_in_shape << std::endl;
        std::cout << "关联路口编号为: ";
        for (const auto& refLink : inter.inter_ref_link) {
            std::cout << refLink << " ";
        }
        std::cout << std::endl;
        std::cout << "路口id为" << inter.inter_id <<"路口形状点个数为" << inter.inter_coords.size() << "个。" << std::endl;
    }
    std::vector<CrosswalkInfo> chaxun_crosswalks = getCrosswalks(crossPath, chaxun_point, distance);
    // 打印查询到的人行横道信息
    for (const auto& cross : chaxun_crosswalks) {
        std::cout << "人行横道id: " << cross.cross_id << std::endl;
    }
    std::vector<TrafficlightInfo> chaxun_trafficlights = getForwardTrafficlight(trafficlightPath, chaxun_point, distance);
    // 打印查询到的信号灯信息
    for (const auto& light : chaxun_trafficlights) {
        std::cout << "信号灯id: " << light.trafficlight_id << std::endl;
        std::cout << "信号灯类型为: " << light.trafficlight_type << std::endl;
        std::cout << "信号灯关联路段编号为: " << light.trafficlight_ref_link << std::endl; 
        std::cout << "信号灯关联车道编号: ";
        for (const auto& refLane : light.trafficlight_ref_lane) {
            std::cout << refLane << " ";
        }
        std::cout << std::endl;
        std::cout << "信号灯指向: ";
        for (const auto& head: light.trafficlight_head) {
            std::cout << head << " ";
        }
        std::cout << std::endl;
        std::cout << "信号灯坐标点个数为: " << light.trafficlight_coords.size() << std::endl;
    }
    std::vector<RoadInfo> chaxun_roads = getLink(roadPath, chaxun_point);
    // 打印查询到的道路信息
    for (const auto& road : chaxun_roads) {
        std::cout << "道路id: " << road.link_id << std::endl;
        std::cout << "道路功能等级为: " << road.link_fc << std::endl;
        std::cout << "道路类型为: " << road.link_type << std::endl;
        std::cout << "道路方向为: " << road.link_direction << std::endl;
        std::cout << "道路最大速度为: " << road.link_speed_max << std::endl;
        std::cout << "道路最小速度为: " << road.link_speed_min << std::endl;
        std::cout << "道路车道数为: " << road.link_lanenum << std::endl;
        std::cout << "道路坐标点个数为: " << road.link_coords.size() << std::endl;
    }
    std::vector<LaneInfo> chaxun_lanes = getLane(lanePath, chaxun_point);
    // 打印查询到的车道信息
    for (const auto& lane : chaxun_lanes) {
        std::cout << "车道id: " << lane.lane_id << std::endl;
        std::cout << "车道长度为: " << lane.lane_length << std::endl;
        std::cout << "车道类型为: " << lane.lane_type << std::endl;
        std::cout << "车道宽度为: " << lane.lane_width << std::endl;
        std::cout << "车道当前行驶方向为: " << lane.lane_turn << std::endl;
        std::cout << "车道关联路段编号为: " << lane.lane_ref_link << std::endl;
        std::cout << "车道最大速度为: " << lane.lane_speed_max << std::endl;
        std::cout << "车道最小速度为: " << lane.lane_speed_min << std::endl;
        std::cout << "车道坐标点个数为: " << lane.lane_coords.size() << std::endl;
    }

    std::vector<SignalInfo> chaxun_signals = getSignals(signPath, chaxun_point, distance);
    // 打印查询到的交通标牌信息
    for (const auto& sign : chaxun_signals) {
        std::cout << "交通标牌id: " << sign.sign_id << std::endl;
        std::cout << "交通标牌类型为: " << sign.sign_type << std::endl;
        std::cout << "交通标牌子类型为" << sign.sign_type1 << std::endl;
        std::cout << "交通标牌关联道路id为: " << sign.sign_ref_link << std::endl;
        std::cout << "交通标牌坐标点个数为: " << sign.sign_coords.size() << std::endl;
    }

    // 根据车道id获取停止线信息
    std::vector<StoplineInfo> chaxun_stoplines = getStopline(stoplinePath, chaxun_point, distance);
    // 打印查询到的停止线信息
    for (const auto& stopline : chaxun_stoplines) {
        std::cout << "停止线id: " << stopline.stopline_id << std::endl;
        std::cout << "停止线关联道路id为: " << stopline.stopline_ref_link << std::endl;
        std::cout << "停止线关联车道编号: ";
        for (const auto& refLane : stopline.stopline_ref_lane) {
            std::cout << refLane << " ";
        }        
        std::cout << std::endl;
        std::cout << "停止线颜色为：" << stopline.stopline_color << std::endl;
        std::cout << "停止线类型为: " << stopline.stopline_type << std::endl;
        std::cout << "停止线坐标点个数为: " << stopline.stopline_coords.size() << std::endl;
    }
    // 根据车道id获取箭头信息
    std::vector<ArrowInfo> chaxun_arrows = getArrow(arrowPath, chaxun_point, distance);
    // 打印查询到的箭头信息
    for (const auto& arrow : chaxun_arrows) {
        std::cout << "箭头id: " << arrow.arrow_id << std::endl;
        std::cout << "箭头关联车道id为: " << arrow.arrow_ref_lane << std::endl;
        std::cout << "箭头方向为: ";
        for (const auto& direction : arrow.arrow_direction) {
            std::cout << direction << " ";
        }
        std::cout << std::endl;
    }
    std::cout << "查询完毕。" << std::endl;
    return 0;
}