#include "pl.hpp"
#include "traffic_light.hpp"

#include <mutex>
#include <condition_variable>

#define WRC_MAX_POINTS_IN_PATH 6000

#define Lat_Origin 30.919389393  // 地面平面坐标系原点的纬度
#define Lon_Origin 112.210442819 // 地面平面坐标系原点的经度
#define Re 6378137
#define Rn 6356755
#define deg_rad (0.01745329252) // Transfer from angle degree to rad
#define R_LATI (6378137)
// #define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
#define R_LONT (5460817) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)

const int limit_stop_time = 20;

double x = 0.0;
double y = 1.0;
int pl_speed = 0;

int station_num;
int GpsPointNum = 0;
WRC_GPS_Point GPSPointQueue[WRC_MAX_POINTS_IN_PATH];
std::vector<StopInfo> stations;
std::vector<StopInfo> stop_line;

std::vector<bool> firstTimeArrivalStation;
std::vector<bool> firstTimeArrivalStopLine;

WRC_MC_NAV_INFO g_NavInfo;
struct_rtk g_rtk;

int mode = 0;
int last_mode = 0;
int command_door = 0; // 1开 2关

std::string filename = "";
int d_file = 0;

Direction drive_mode = Direction::STRAIGHT_D;
int slowdown_flag = 0; // 1开始减速 0恢复速度
int stop_flag = 0;     // 1停车 0不停车

// int id_route = 1;
int id_station = 1;
int id_stop = 1;

// double aim_x[19];
// double aim_y[19];

// 内圈：e6f09885791749d8a26968c21a5d3905
// 外圈：8b55528fb27c44f7a1b2d56d5d614ddf
std::string routeId = "e6f09885791749d8a26968c21a5d3905";

std::mutex mtx;
std::condition_variable cv;
bool matched = false;

// 返回单位：m
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
{
    double x, y, length;
    x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式
    y = (R_LATI) * (lati2 - lati1) * deg_rad;
    length = sqrt(x * x + y * y);
    return length;
}

// Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m
int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, double *x_diff, double *y_diff)
{
    double Navi_rad, x, y;
    float k1, k2, k3, k4;              // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta);
    Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度
    // 以当前位置为原点，以正北为y正轴，以正东为x正轴；
    x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT;
    y = (Dest_lati - Cur_lati) * deg_rad * R_LATI;
    k1 = cos(Navi_rad);
    k2 = (-1) * sin(Navi_rad);
    k3 = (-1) * k2;
    k4 = k1;
    // 以当前航向角作为旋转角
    *x_diff = x * k1 + y * k2;
    *y_diff = x * k3 + y * k4;

    return 0;
}

// 匹配最近的点

int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info)
{
    if (start_index < 0)
    {
        return -1;
    }

    int Nearest_point_index = start_index; // 初始化为起始索引
    double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
                                          Road_info[start_index].LongitudeInfo,
                                          Road_info[start_index].LatitudeInfo);

    // 从 start_index + 1 开始搜索
    for (int i = 1; i < GpsPointNum; i++)
    {
        int current_index = (start_index + i) % GpsPointNum; // 环绕访问

        double current_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
                                                  Road_info[current_index].LongitudeInfo,
                                                  Road_info[current_index].LatitudeInfo);

        if (current_distance < min_distance)
        {
            min_distance = current_distance;
            Nearest_point_index = current_index; // 更新最近点的索引

            // 如果距离小于 0.8 m，提前退出
            if (min_distance < 0.8)
            {
                return Nearest_point_index;
            }
        }
    }

    return Nearest_point_index; // 返回最近点的索引
}
/*
int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info)
{

    if (start_index < 0)
    {
        return -1;
    }
    int Nearest_point_index = 0;
    double aft_distance = 0, pre_distance = 0;
    pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);

    int start = start_index + 1;
    for (int i = start; i < GpsPointNum; i++)
    {

        aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo);
        // printf("i: %d , aft_distance: %lf \t",i,aft_distance);
        if (aft_distance < pre_distance)
        {
            pre_distance = aft_distance;
            continue;
        }
        else
        {
            Nearest_point_index = i - 1;

            return Nearest_point_index;
        }
    }
    return GpsPointNum - 1;
}
*/

// 找到目标点
int Road_Planning_Find_Aim_Point(int start_index, int dest_num)
{
    int dest_index = start_index;
    if (start_index + dest_num <= (GpsPointNum - 1))
    {
        dest_index = start_index + dest_num;
    }
    else
    {
        dest_index = (GpsPointNum - 1);
    }

    return (dest_index);
}

int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info)
{
    double dis = 10000;
    int point = 0;
    for (int i = 0; i < GpsPointNum; i++)
    {
        double dis_temp = ntzx_GPS_length(lon, lat, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo);
        if (dis_temp < dis)
        {
            dis = dis_temp;
            point = i;
        }
        printf("i: %d , dis_temp: %lf", i, dis_temp);
    }
    return point;
}

// 判断车辆是否在左转、右转还是直行
Direction straight_or_turn(double cur_direction, double des_direction, int threshold)
{
    double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0);

    if (diff > 180)
        diff = 360 - diff;

    // std::cout << "diff : " << diff << std::endl;

    if (diff < threshold)
    {
        return Direction::STRAIGHT_D;
    }
    else
    {
        // 计算角度差的符号来确定转向方向
        double angle_diff = std::fmod(des_direction - cur_direction + 360.0, 360.0);

        if (angle_diff > 180)
            return Direction::LEFT_TURN;
        else
            return Direction::RIGHT_TURN;
    }
}

// ====================================================================================================
// 根据灯色和位置处理交通信号灯
void handleTrafficLightColor(int color, const std::string &position)
{
    switch (color)
    {
    case 1: // RED
    case 2: // YELLOW
        if (position == "减速")
        {
            slowdown_flag = 1;
            std::cout << "==================开始减速" << std::endl;
        }
        else if (position == "停车")
        {
            // pl_speed = 0;
            stop_flag = 1;
            std::cout << "==================停车" << std::endl;
        }
        break;
    case 3: // GREEN
        if (position == "减速")
        {
            slowdown_flag = 0;
            std::cout << "==================不减速" << std::endl;
        }

        else if (position == "停车")
        {
            // pl_speed = 4;
            stop_flag = 0;
            std::cout << "================== 不停车" << std::endl;
        }

        break;
    default:
        std::cout << "Unknown color" << std::endl;
        break;
    }
}

// 根据方向、位置和信号灯状态选择和处理交通信号灯
//   因为模型只能检测颜色，不支持检测类型，所以简单以位置判断
void handleTrafficLight(Direction direction, const std::vector<TrafficLight> &trafficLights, const std::string &position)
{

    // if ("red/yellow")
    //     slowdown_flag = 1;
    // else
    //     slowdown_flag = 0;

    // 判断trafficLights是否为空，避免访问越界
    if (trafficLights.empty())
        return;

    int index = -1;

    // 根据不同方向选择对应的交通灯
    switch (direction)
    {
    case Direction::STRAIGHT_D:
        index = (trafficLights.size() == 3) ? 1 : 0; // 选择第二个或第一个交通灯
        break;

    case Direction::LEFT_TURN:
        index = 0; // 左转处理第一个交通灯
        break;

    case Direction::RIGHT_TURN:
        index = (trafficLights.size() == 3) ? 2 : 0; // 选择第三个或第一个交通灯
        break;
    }
    // 处理选择的交通灯
    if (index >= 0 && index < static_cast<int>(trafficLights.size()))
    {
        const auto &light = trafficLights[index];
        handleTrafficLightColor(light.color, position);
    }
}

void handlePosition(const StopInfo &info, int road_pos, const std::string &action)
{
    const int tolerance = 5; // 定义一个允许的误差范围，例如 2 个单位
    // if (std::abs(road_pos - info.slowdown_pos) <= tolerance)
    // else if (std::abs(road_pos - info.stop_pos) <= tolerance)
    // else if (std::abs(road_pos - info.resume_pos) <= tolerance)

    if (action == "进站 : ")
    {
        if (road_pos == info.slowdown_pos)
        {
            std::cout << action << "到达减速位置" << std::endl;

            slowdown_flag = 1;
        }
        else if (road_pos == info.stop_pos)
        {
            std::cout << action << "到停车位置" << std::endl;

            // pl_speed = 0;
            stop_flag = 1;
            command_door = 1;
            for (int st = 0; st < limit_stop_time; st++)
                sleep(1);
            command_door = 2;
            // pl_speed = 4;
            stop_flag = 0;
        }
        else if (road_pos >= info.resume_pos)
        {
            std::cout << action << "恢复正常速" << std::endl;

            slowdown_flag = 0;
            firstTimeArrivalStation[id_station - 1] = false;
            id_station++;
        }
    }

    if (action == "路口 : ")
    {
        if (road_pos == info.slowdown_pos)
        {
            std::cout << action << "到达减速位置" << std::endl;

            handleTrafficLight(drive_mode, ros_traffic_light, "减速");
        }
        else if ((road_pos - info.stop_pos) <= tolerance && (road_pos - info.stop_pos) > 0)
        {
            std::cout << action << "到停车位置" << std::endl;

            handleTrafficLight(drive_mode, ros_traffic_light, "停车");
        }
        else if (road_pos >= info.resume_pos)
        {
            std::cout << action << "恢复正常速" << std::endl;

            slowdown_flag = 0;
            firstTimeArrivalStopLine[id_stop - 1] = false;
            id_stop++;
        }
    }

    /*
    if (road_pos == info.slowdown_pos)
    {
        std::cout << action << "到达减速位置" << std::endl;

        if (action == "进站 : ")
            slowdown_flag = 1;

        if (action == "路口 : ")
            handleTrafficLight(drive_mode, ros_traffic_light, "减速");
    }

    else if (road_pos == info.stop_pos)
    {
        std::cout << action << "到停车位置" << std::endl;

        if (action == "进站 : ")
        {
            pl_speed = 0;
            command_door = 1;
            for (int st = 0; st < limit_stop_time; st++)
                sleep(1);
            command_door = 2;
            pl_speed = 4;
        }

        if (action == "路口 : ")
            handleTrafficLight(drive_mode, ros_traffic_light, "停车");
    }

    else if (road_pos >= info.resume_pos)
    {
        std::cout << action << "恢复正常速" << std::endl;

        if (action == "进站 : ")
        {
            slowdown_flag = 0;

            firstTimeArrivalStation[id_station - 1] = false;
            id_station++;
        }

        if (action == "路口 : ")
        {
            slowdown_flag = 0;

            firstTimeArrivalStopLine[id_stop - 1] = false;
            id_stop++;
        }
    }
    */
}

// 根据当前位置执行对应操作
void matchAndExecute(const std::vector<StopInfo> &stations, const std::vector<StopInfo> &stops, int road_pos)
{
    // std::cout << "下一站台位置 : " << id_station << std::endl;
    // for (int i = 0; i < firstTimeArrivalStation.size(); i++)
    //     std::cout << " " << firstTimeArrivalStation[i] << std::endl;

    // std::cout << "下一stop位置 : " << id_stop << std::endl;
    // for (int i = 0; i < firstTimeArrivalStopLine.size(); i++)
    //     std::cout << " " << firstTimeArrivalStopLine[i] << std::endl;

    if (id_station - 1 < static_cast<int>(firstTimeArrivalStation.size())) // 防止数组越界
    {
        if (firstTimeArrivalStation[id_station - 1])
        {
            handlePosition(stations[id_station - 1], road_pos, "进站 : ");
        }
    }

    if (id_stop - 1 < static_cast<int>(firstTimeArrivalStopLine.size()))
    {
        if (firstTimeArrivalStopLine[id_stop - 1])
        {
            handlePosition(stops[id_stop - 1], road_pos, "路口 : ");
        }
    }
}
// ====================================================================================================

// 读取txt信息
std::vector<StopInfo> readTXT(const std::string &filename)
{
    std::vector<StopInfo> info;
    std::ifstream file(filename);
    if (file.is_open())
    {
        std::cout << "have opened " << filename << std::endl;

        int slowdown, stop, resume;
        while (file >> slowdown >> stop >> resume)
        {
            info.push_back({slowdown, stop, resume});
            std::cout << "slowdown : " << slowdown
                      << " stop : " << stop
                      << " resume : " << resume
                      << std::endl;
        }
        file.close();
        std::cout << std::endl;
    }
    else
    {
        std::cerr << "Unable to open " << filename << std::endl;
    }

    return info;
}

// 读取寻迹信息
void readGPSPoints(const std::string &filename)
{
    FILE *fp_gps = fopen(filename.c_str(), "r");
    if (fp_gps == NULL)
    {
        std::cerr << "Unable to open gps.txt" << std::endl;
        return;
    }
    std::cout << "have opened " << filename << std::endl;
    char mystring[200];
    int i = 0;
    while (fgets(mystring, sizeof(mystring), fp_gps))
    {
        std::string line_string(mystring);
        int point_num = i / 4;

        if (i % 4 == 0)
        {
            GPSPointQueue[point_num].LatitudeInfo = atof(line_string.c_str());
        }
        else if (i % 4 == 1)
        {
            GPSPointQueue[point_num].LongitudeInfo = atof(line_string.c_str());
        }
        else if (i % 4 == 2)
        {
            GPSPointQueue[point_num].Heading = atof(line_string.c_str());
        }
        i++;
    }

    GpsPointNum = i / 4;
    printf("point_num:%d\n", GpsPointNum);
    fclose(fp_gps);
    std::cout << std::endl;
}

void PL_ProcThread()
{
    usleep(70000);

    int road_pos = 0;
    int des_pos = 5;
    int direction_pos = 12;

    while (1)
    {
        // 更新导航信息
        g_NavInfo.Latitude_degree = g_rtk.lat;
        g_NavInfo.Longitude_degree = g_rtk.lon;
        g_NavInfo.Yaw_rad = g_rtk.direction;

        // 检查RTK定位质量，如果不可靠则停车
        if (g_rtk.reliability == 0)
        {
            // 确保车辆停止
            x = 0.0;
            y = 1.0;
            pl_speed = 0;
            printf("RTK定位质量为0，已停车\n"); // 增加调试信息
            sleep(1);                           // 缩短休眠时间，以便更快检测到恢复
            continue;
        }

        // 只有RTK正常时才进行路径规划
        road_pos = Road_Planning_Find_Nearest_Point(des_pos - 5, g_NavInfo, GPSPointQueue);
        des_pos = Road_Planning_Find_Aim_Point(road_pos, 5);        // 找到当前点后第5个点，作为目标点
        direction_pos = Road_Planning_Find_Aim_Point(road_pos, 12); // 找到当前点后第12个点，判断直行/转弯

        printf("当前位置 : %d\n", road_pos);
        printf("下一位置 : %d\n", des_pos);

        drive_mode = straight_or_turn(g_rtk.direction, GPSPointQueue[direction_pos].Heading, 12);

        // 检查是否到达终点
        if (road_pos >= (GpsPointNum - 2) || des_pos == -1)
        {
            x = 0.0;
            y = 1.0;
            pl_speed = 0;
            sleep(3);
            continue;
        }
        else if (des_pos != -1)
        {
            double x_cm_tmp = 0;
            double y_cm_tmp = 0;
            ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat,
                           GPSPointQueue[des_pos].LongitudeInfo,
                           GPSPointQueue[des_pos].LatitudeInfo,
                           &x_cm_tmp, &y_cm_tmp);

            x = x_cm_tmp;
            y = y_cm_tmp;

            if (stop_flag == 1)
                pl_speed = 0;
            else
                pl_speed = 4;
        }
    }

    return;
}

// void pl_thread(void)
void *pl_thread(void *args)
{
    (void)args;
    sleep(3);

#ifdef ZXWL

    // 读取文件
    memset(GPSPointQueue, 0, sizeof(GPSPointQueue));
    stations = readTXT("common/txt/stations.txt"); // 读取站台信息
    stop_line = readTXT("common/txt/stop.txt");    // 读取停止线信息
    readGPSPoints("common/txt/gps_load_now.txt");  // 读取路线信息

    std::cout << "站台个数 : " << stations.size() << std::endl;
    std::cout << "停止线个数 : " << stop_line.size() << std::endl;

    firstTimeArrivalStation.resize(stations.size());
    std::fill(firstTimeArrivalStation.begin(), firstTimeArrivalStation.end(), true);

    firstTimeArrivalStopLine.resize(stop_line.size());
    std::fill(firstTimeArrivalStopLine.begin(), firstTimeArrivalStopLine.end(), true);

#else

    // 循环等待下发路径之后再继续
    // while (d_file != 1)
    // {
    //     printf("Waiting for the route to be issued!\n");
    //     sleep(1);
    // }
    // printf("#################### Received delivery path! ####################\n");
    // 读取文件
    memset(GPSPointQueue, 0, sizeof(GPSPointQueue));
    stations = readTXT("common/txt/stations.txt");
    stop_line = readTXT("common/txt/stop.txt");

    std::cout << "站台个数 : " << stations.size() << std::endl;
    std::cout << "停止线个数 : " << stop_line.size() << std::endl;

    firstTimeArrivalStation.resize(stations.size());
    std::fill(firstTimeArrivalStation.begin(), firstTimeArrivalStation.end(), true);

    firstTimeArrivalStopLine.resize(stop_line.size());
    std::fill(firstTimeArrivalStopLine.begin(), firstTimeArrivalStopLine.end(), true);

    std::cout << "all true : " << std::endl;

    // 判断 routeId
    if (routeId == "e6f09885791749d8a26968c21a5d3905")
    {
        readGPSPoints("common/txt/gps_load_nei.txt");
    }
    else if (routeId == "8b55528fb27c44f7a1b2d56d5d614ddf")
    {
        readGPSPoints("common/txt/gps_load_wai.txt");
    }
    else
    {
        readGPSPoints("common/txt/gps_load_nei.txt");
    }

#endif // #ifdef ZXWL

    while (mode != 1)
    {
        sleep(1);
    }
    printf("####################     auto drive on!      ####################\n");

    PL_ProcThread();

    return nullptr;
}

// for (int i = 0; i < 19; i++)
// {
//     aim_x[i] = 0.0;
// }
// for (int i = 0; i < 19; i++)
// {
//     aim_y[i] = (double)(i + 3);
// }

// printf("x_cm_tmp: %lf \n", x_cm_tmp);
// printf("y_cm_tmp: %lf \n", y_cm_tmp);
// printf("head_raw: %lf \n", head_raw);

// for (int i = 1; i < 20; i++) // 目标点之后的19个点
// {
//     int radar_pos = des_pos + i;

//     // double radar_x_tmp = 0.0;
//     // double radar_y_tmp = 0.0;

//     ntzx_GPS_posit(head_raw, g_NavInfo.Longitude_degree, g_NavInfo.Latitude_degree,
//                    GPSPointQueue[radar_pos].LongitudeInfo,
//                    GPSPointQueue[radar_pos].LatitudeInfo,
//                    &aim_x[i], &aim_y[i]);
// }

// void handleTrafficLight(Direction direction, const std::vector<TrafficLight> &trafficLights, const std::string &position)
// {
//     for (const auto &light : trafficLights)
//     {
//         bool isValidLightType = false;

//         switch (direction)
//         {
//         case Direction::STRAIGHT_D:
//             isValidLightType = (light.type == LightType::STRAIGHT_L ||
//                                 light.type == LightType::STRAIGHT_TURN_LEFT ||
//                                 light.type == LightType::STRAIGHT_TURN_RIGHT ||
//                                 light.type == LightType::CIRCULAR);
//             break;

//         case Direction::LEFT_TURN:
//             isValidLightType = (light.type == LightType::TURN_LEFT ||
//                                 light.type == LightType::STRAIGHT_TURN_LEFT);
//             break;

//         case Direction::RIGHT_TURN:
//             isValidLightType = (light.type == LightType::TURN_RIGHT ||
//                                 light.type == LightType::STRAIGHT_TURN_RIGHT ||
//                                 light.type == LightType::CIRCULAR);
//             break;

//         default:
//             std::cout << "Unknown direction." << std::endl;
//             continue;
//         }

//         if (isValidLightType)
//         {
//             handleTrafficLightColor(light.color, position);
//         }
//     }
// }

// 这逻辑不对，如果是直线那没问题，但是对于环形道路，最近点的搜索需要考虑回绕的情况。
// int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info)
// {

//     if (start_index < 0)
//     {
//         return -1;
//     }
//     int Nearest_point_index = 0;
//     double aft_distance = 0, pre_distance = 0;
//     pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);

//     int start = start_index + 1;
//     for (int i = start; i < GpsPointNum; i++)
//     {

//         aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo);
//         // printf("i: %d , aft_distance: %lf \t",i,aft_distance);
//         if (aft_distance < pre_distance)
//         {
//             pre_distance = aft_distance;
//             continue;
//         }
//         else
//         {
//             Nearest_point_index = i - 1;

//             return Nearest_point_index;
//         }
//     }
//     return GpsPointNum - 1;
// }