#include <iostream>
#include <sstream>
#include "engine.h"
#include "csv.h"
#include <string>
#include <cmath>
#include <iomanip>
#include <fstream>
#include "utils.h"
#include <map>
#include <algorithm>
#include <vector>
const double X_OFFSET = 258109.4239876;
const double Y_OFFSET = 4149969.964821;

int main(int argc, char **argv)
{

    std::string mapCsv_path = "/home/kevin/kevin/zhaoyuan/zhaoyuan/models/map/data";
    std::string csv_data_path = "/home/kevin/kevin/zhaoyuan/zhaoyuan/data/processed/data";
    std::string csv_read_data = csv_data_path + "/ObjState.csv";
    std::string csv_writedata = csv_data_path + "/EgoMap.csv";

    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";

    // 创建一个 CSV 解析器对象 招远xodr地图原点坐标
    // xOffset value=258109.4239876
    // yOffset value=4149969.964821
    io::CSVReader<6> reader(csv_read_data); // 假设有 3 列
    reader.read_header(io::ignore_extra_column, "simTime", "simFrame", "playerId", "posX", "posY", "posH");

    io::CSVWriter<11> writer(csv_writedata, {"simTime", "simFrame", "playerId", "posX", "posY", "posH", "road_link_id", "road_fc", "road_type", "road_speed_max", "road_speed_min"});

    int simFrame;
    int playerId;
    double simTime;
    double posX;
    double posY;
    double posH;
    //     struct RoadInfo
    // {
    //     int link_id;
    //     int link_fc;
    //     int link_type;
    //     int link_speed_max;
    //     int link_speed_min;
    //     int link_direction;
    //     int link_lanenum;
    //     std::vector<Point> link_coords;
    // };

    while (reader.read_row(simTime, simFrame, playerId, posX, posY, posH))
    {
        if (playerId == 1)
        {
            // std::cout << "Row: " << simTime << " " << simFrame << " " << posX << " " << posY << " " << posH << std::endl;
            posX += X_OFFSET;
            posY += Y_OFFSET;
            Point point(posX, posY, posH);
            double distance = 1;

            std::vector<RoadInfo> chaxun_roads = getLink(roadPath, point);
            // std::vector<InterInfo> chaxun_inters = getInters(interPath, 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;
            // }
            for (auto road : chaxun_roads)
            {
                // std::cout << "road link_id: " << road.link_id << std::endl;
                // std::cout << "road link_fc: " << road.link_fc << std::endl;
                // std::cout << "road link_type: " << road.link_type << std::endl;
                // std::cout << "road link_speed_max: " << road.link_speed_max << std::endl;
                // std::cout << "road link_speed_min: " << road.link_speed_min << std::endl;
                int id = 1;

                writer.write_row(simTime, simFrame, id, posX, posY, posH, road.link_id, road.link_fc, road.link_type, road.link_speed_max, road.link_speed_min);
            }
        }
    }

    return 0;
}