zhaoyuan/modules/metric/function.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
##################################################################
#
# Copyright (c) 2025 CICV, Inc. All Rights Reserved
#
##################################################################
"""
@Authors:           zhanghaiwen(zhanghaiwen@china-icv.cn)
@Data:              2025/01/5
@Last Modified:     2025/01/5
@Summary:           Function Metrics Calculation
"""

import sys
from pathlib import Path

# 添加项目根目录到系统路径
root_path = Path(__file__).resolve().parent.parent
sys.path.append(str(root_path))
print(root_path)

from modules.lib.score import Score
from modules.lib.log_manager import LogManager
import numpy as np
from typing import Dict, Tuple, Optional, Callable, Any
import pandas as pd
import yaml
from modules.lib.chart_generator import generate_function_chart_data
from shapely.geometry import Point, Polygon
from modules.lib.common import get_interpolation

# ----------------------
# 基础工具函数 (Pure functions)
# ----------------------
scenario_sign_dict = {"LeftTurnAssist": 206, "HazardousLocationW": 207, "RedLightViolationW": 208,
                      "CoorperativeIntersectionPassing": 225, "GreenLightOptimalSpeedAdvisory": 234,
                      "ForwardCollision": 212}


def _is_pedestrian_in_crosswalk(polygon, test_point) -> bool:
    polygon = Polygon(polygon)
    point = Point(test_point)
    return polygon.contains(point)


def _is_segment_by_interval(time_list, expected_step) -> list:
    """
    根据时间戳之间的间隔进行分段。

    参数:
    time_list (list): 时间戳列表。
    expected_step (float): 预期的固定步长。

    返回:
    list: 分段后的时间戳列表，每个元素是一个子列表。
    """
    if not time_list:
        return []

    segments = []
    current_segment = [time_list[0]]

    for i in range(1, len(time_list)):
        actual_step = time_list[i] - time_list[i - 1]
        if actual_step != expected_step:
            # 如果间隔不符合预期，则开始一个新的段
            segments.append(current_segment)
            current_segment = [time_list[i]]
        else:
            # 否则，将当前时间戳添加到当前段中
            current_segment.append(time_list[i])

    # 添加最后一个段
    if current_segment:
        segments.append(current_segment)

    return segments


# 寻找二级指标的名称
def find_nested_name(data):
    """
    查找字典中嵌套的name结构。

    :param data: 要搜索的字典
    :return: 找到的第一个嵌套name结构的值，如果没有找到则返回None
    """
    if isinstance(data, dict):
        for key, value in data.items():
            if isinstance(value, dict) and 'name' in value:
                return value['name']
            # 递归查找嵌套字典
            result = find_nested_name(value)
            if result is not None:
                return result
    elif isinstance(data, list):
        for item in data:
            result = find_nested_name(item)
            if result is not None:
                return result
    return None


def calculate_distance_PGVIL(ego_pos: np.ndarray, obj_pos: np.ndarray) -> np.ndarray:
    """向量化距离计算"""
    return np.linalg.norm(ego_pos - obj_pos, axis=1)


def calculate_relative_speed_PGVIL(
        ego_speed: np.ndarray, obj_speed: np.ndarray
) -> np.ndarray:
    """向量化相对速度计算"""
    return np.linalg.norm(ego_speed - obj_speed, axis=1)


def calculate_distance(ego_df: pd.DataFrame, correctwarning: int) -> np.ndarray:
    """向量化距离计算"""
    dist = ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())]['relative_dist']
    return dist


def calculate_relative_speed(ego_df: pd.DataFrame, correctwarning: int) -> np.ndarray:
    """向量化相对速度计算"""
    return ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())]['composite_v']


def extract_ego_obj(data: pd.DataFrame) -> Tuple[pd.Series, pd.DataFrame]:
    """数据提取函数"""
    ego = data[data["playerId"] == 1].iloc[0]
    obj = data[data["playerId"] != 1]
    return ego, obj


def get_first_warning(data_processed) -> Optional[pd.DataFrame]:
    """带缓存的预警数据获取"""
    ego_df = data_processed.ego_data
    obj_df = data_processed.object_df

    scenario_name = find_nested_name(data_processed.function_config["function"])
    correctwarning = scenario_sign_dict.get(scenario_name)

    if correctwarning is None:
        print("无法获取正确的预警信号标志位！")
        return None
    warning_rows = ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())]

    warning_times = warning_rows['simTime']
    if warning_times.empty:
        print("没有找到预警数据！")
        return None

    first_time = warning_times.iloc[0]
    return obj_df[obj_df['simTime'] == first_time]


# ----------------------
# 核心计算功能函数
# ----------------------
def latestWarningDistance_LST(data) -> dict:
    """预警距离计算流水线"""
    scenario_name = find_nested_name(data.function_config["function"])
    value = data.function_config["function"][scenario_name]["latestWarningDistance_LST"]["max"]
    correctwarning = scenario_sign_dict[scenario_name]
    ego_df = data.ego_data
    warning_dist = calculate_distance(ego_df, correctwarning)
    warning_speed = calculate_relative_speed(ego_df, correctwarning)

    # 将计算结果保存到data对象中，供图表生成使用
    data.warning_dist = warning_dist
    data.warning_speed = warning_speed
    data.correctwarning = correctwarning

    if warning_dist.empty:
        return {"latestWarningDistance_LST": 0.0}

    # 生成图表数据
    generate_function_chart_data(data, 'latestWarningDistance_LST')

    return {"latestWarningDistance_LST": float(warning_dist.iloc[-1]) if len(warning_dist) > 0 else value}


def earliestWarningDistance_LST(data) -> dict:
    """预警距离计算流水线"""
    scenario_name = find_nested_name(data.function_config["function"])
    value = data.function_config["function"][scenario_name]["earliestWarningDistance_LST"]["max"]
    correctwarning = scenario_sign_dict[scenario_name]
    ego_df = data.ego_data
    warning_dist = calculate_distance(ego_df, correctwarning)
    warning_speed = calculate_relative_speed(ego_df, correctwarning)

    # 将计算结果保存到data对象中，供图表生成使用
    data.warning_dist = warning_dist
    data.warning_speed = warning_speed
    data.correctwarning = correctwarning

    if warning_dist.empty:
        return {"earliestWarningDistance_LST": 0.0}

    # 生成图表数据

    generate_function_chart_data(data, 'earliestWarningDistance_LST')

    return {"earliestWarningDistance_LST": float(warning_dist.iloc[0]) if len(warning_dist) > 0 else value}


def latestWarningDistance_TTC_LST(data) -> dict:
    """TTC计算流水线"""
    scenario_name = find_nested_name(data.function_config["function"])
    value = data.function_config["function"][scenario_name]["latestWarningDistance_TTC_LST"]["max"]
    correctwarning = scenario_sign_dict[scenario_name]
    ego_df = data.ego_data
    warning_dist = calculate_distance(ego_df, correctwarning)
    if warning_dist.empty:
        return {"latestWarningDistance_TTC_LST": 0.0}

    # 将correctwarning保存到data对象中，供图表生成使用
    data.correctwarning = correctwarning

    warning_speed = calculate_relative_speed(ego_df, correctwarning)

    with np.errstate(divide='ignore', invalid='ignore'):
        ttc = np.where(warning_speed != 0, warning_dist / warning_speed, np.inf)

    # 处理无效的TTC值
    for i in range(len(ttc)):
        ttc[i] = float(value) if (not ttc[i] or ttc[i] < 0) else ttc[i]

    data.warning_dist = warning_dist
    data.warning_speed = warning_speed
    data.ttc = ttc
    # 生成图表数据
    # from modules.lib.chart_generator import generate_function_chart_data
    generate_function_chart_data(data, 'latestWarningDistance_TTC_LST')

    return {"latestWarningDistance_TTC_LST": float(ttc[-1]) if len(ttc) > 0 else value}


def earliestWarningDistance_TTC_LST(data) -> dict:
    """TTC计算流水线"""
    scenario_name = find_nested_name(data.function_config["function"])
    value = data.function_config["function"][scenario_name]["earliestWarningDistance_TTC_LST"]["max"]
    correctwarning = scenario_sign_dict[scenario_name]
    ego_df = data.ego_data
    warning_dist = calculate_distance(ego_df, correctwarning)
    if warning_dist.empty:
        return {"earliestWarningDistance_TTC_LST": 0.0}

    # 将correctwarning保存到data对象中，供图表生成使用
    data.correctwarning = correctwarning

    warning_speed = calculate_relative_speed(ego_df, correctwarning)

    with np.errstate(divide='ignore', invalid='ignore'):
        ttc = np.where(warning_speed != 0, warning_dist / warning_speed, np.inf)

    # 处理无效的TTC值
    for i in range(len(ttc)):
        ttc[i] = float(value) if (not ttc[i] or ttc[i] < 0) else ttc[i]

    # 将计算结果保存到data对象中，供图表生成使用
    data.warning_dist = warning_dist
    data.warning_speed = warning_speed
    data.ttc = ttc
    data.correctwarning = correctwarning

    # 生成图表数据
    generate_function_chart_data(data, 'earliestWarningDistance_TTC_LST')

    return {"earliestWarningDistance_TTC_LST": float(ttc[0]) if len(ttc) > 0 else value}


def warningDelayTime_LST(data):
    scenario_name = find_nested_name(data.function_config["function"])
    correctwarning = scenario_sign_dict[scenario_name]
    # 将correctwarning保存到data对象中，供图表生成使用
    data.correctwarning = correctwarning
    ego_df = data.ego_data
    HMI_warning_rows = ego_df[(ego_df['ifwarning'] == correctwarning)]['simTime'].tolist()
    simTime_HMI = HMI_warning_rows[0] if len(HMI_warning_rows) > 0 else None
    rosbag_warning_rows = ego_df[(ego_df['event_Type'].notna()) & ((ego_df['event_Type'] != np.nan))][
        'simTime'].tolist()
    simTime_rosbag = rosbag_warning_rows[0] if len(rosbag_warning_rows) > 0 else None
    if (simTime_HMI is None) or (simTime_rosbag is None):
        print("预警出错！")
        delay_time = 100.0
    else:
        delay_time = abs(simTime_HMI - simTime_rosbag)
    return {"warningDelayTime_LST": delay_time}


def warningDelayTimeofReachDecel_LST(data):
    scenario_name = find_nested_name(data.function_config["function"])
    correctwarning = scenario_sign_dict[scenario_name]
    # 将correctwarning保存到data对象中，供图表生成使用
    data.correctwarning = correctwarning
    ego_df = data.ego_data
    obj_df = data.obj_data
    obj_speed_simtime = obj_df[obj_df['accel'] <= -4]['simTime'].tolist()  # 单位m/s^2
    warning_simTime = ego_df[ego_df['ifwarning'] == correctwarning]['simTime'].tolist()
    if (len(warning_simTime) == 0) and (len(obj_speed_simtime) == 0):
        return {"warningDelayTimeofReachDecel_LST": 0}
    elif (len(warning_simTime) == 0) and (len(obj_speed_simtime) > 0):
        return {"warningDelayTimeofReachDecel_LST": obj_speed_simtime[0]}
    elif (len(warning_simTime) > 0) and (len(obj_speed_simtime) == 0):
        return {"warningDelayTimeofReachDecel_LST": None}
    else:
        return {"warningDelayTimeofReachDecel_LST": warning_simTime[0] - obj_speed_simtime[0]}


def rightWarningSignal_LST(data):
    scenario_name = find_nested_name(data.function_config["function"])
    correctwarning = scenario_sign_dict[scenario_name]
    # 将correctwarning保存到data对象中，供图表生成使用
    data.correctwarning = correctwarning
    ego_df = data.ego_data
    if ego_df['ifwarning'].empty:
        print("无法获取正确预警信号标志位！")
        return
    warning_rows = ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['ifwarning'].notna())]
    if warning_rows.empty:
        return {"rightWarningSignal_LST": -1}
    else:
        return {"rightWarningSignal_LST": 1}


def ifCrossingRedLight_LST(data):
    scenario_name = find_nested_name(data.function_config["function"])
    correctwarning = scenario_sign_dict[scenario_name]
    # 将correctwarning保存到data对象中，供图表生成使用
    data.correctwarning = correctwarning
    ego_df = data.ego_data
    redlight_simtime = ego_df[
        (ego_df['ifwarning'] == correctwarning) & (ego_df['stateMask'] == 1) & (ego_df['relative_dist'] == 0) & (
                ego_df['v'] != 0)]['simTime']
    if redlight_simtime.empty:
        return {"ifCrossingRedLight_LST": -1}
    else:
        return {"ifCrossingRedLight_LST": 1}


def ifStopgreenWaveSpeedGuidance_LST(data):
    scenario_name = find_nested_name(data.function_config["function"])
    correctwarning = scenario_sign_dict[scenario_name]
    # 将correctwarning保存到data对象中，供图表生成使用
    data.correctwarning = correctwarning
    ego_df = data.ego_data
    greenlight_simtime = \
        ego_df[(ego_df['ifwarning'] == correctwarning) & (ego_df['stateMask'] == 0) & (ego_df['v'] == 0)]['simTime']
    if greenlight_simtime.empty:
        return {"ifStopgreenWaveSpeedGuidance_LST": -1}
    else:
        return {"ifStopgreenWaveSpeedGuidance_LST": 1}


# ------ 单车智能指标 ------
def limitSpeed_LST(data):
    ego_df = data.ego_data
    scenario_name = find_nested_name(data.function_config["function"])
    limit_speed = data.function_config["function"][scenario_name]["limitSpeed_LST"]["max"]
    speed_limit = ego_df[abs(ego_df['x_relative_dist']) <= 0.1]['v'].tolist()
    if len(speed_limit) == 0:
        return {"speedLimit_LST": -1}
    max_speed = max(speed_limit)
    data.speedLimit = limit_speed
    generate_function_chart_data(data, 'limitspeed_LST')
    return {"speedLimit_LST": max_speed}


def limitSpeedPastLimitSign_LST(data):
    ego_df = data.ego_data
    scenario_name = find_nested_name(data.function_config["function"])
    limit_speed = data.function_config["function"][scenario_name]["limitSpeed_LST"]["max"]
    car_length = data.function_config["vehicle"]['CAR_LENGTH']
    ego_speed = ego_df[ego_df['x_relative_dist'] <= -100 - car_length]['v'].tolist()
    ego_time = ego_df[ego_df['x_relative_dist'] <= -100 - car_length]['simTime'].tolist()
    data.speedLimit = limit_speed
    data.speedPastLimitSign_LST = ego_time[0] if len(ego_time) > 0 else None
    generate_function_chart_data(data, 'limitSpeedPastLimitSign_LST')
    if len(ego_speed) == 0:
        return {"speedPastLimitSign_LST": -1}
    return {"speedPastLimitSign_LST": ego_speed[0]}


def leastDistance_LST(data):
    ego_df = data.ego_data
    dist_row = ego_df[ego_df['v'] == 0]['relative_dist'].tolist()
    if len(dist_row) == 0:
        return {"leastDistance_LST": -1}
    else:
        min_dist = min(dist_row)
        return {"leastDistance_LST": min_dist}


def launchTimeinStopLine_LST(data):
    ego_df = data.ego_data
    simtime_row = ego_df[ego_df['v'] == 0]['simTime'].tolist()
    if len(simtime_row) == 0:
        return {"launchTimeinStopLine_LST": -1}
    else:
        delta_t = simtime_row[-1] - simtime_row[0]
        return {"launchTimeinStopLine_LST": delta_t}


def launchTimewhenFollowingCar_LST(data):
    ego_df = data.ego_data
    t_interval = ego_df['simTime'].tolist()[1] - ego_df['simTime'].tolist()[0]
    simtime_row = ego_df[ego_df['v'] == 0]['simTime'].tolist()
    if len(simtime_row) == 0:
        return {"launchTimewhenFollowingCar_LST": 0}
    else:
        time_interval = _is_segment_by_interval(simtime_row, t_interval)
        delta_t = []
        for t in time_interval:
            delta_t.append(t[-1] - t[0])
        return {"launchTimewhenFollowingCar_LST": max(delta_t)}


def noStop_LST(data):
    ego_df_ini = data.ego_data
    min_time = ego_df_ini['simTime'].min() + 5
    max_time = ego_df_ini['simTime'].max() - 5
    ego_df = ego_df_ini[(ego_df_ini['simTime'] >= min_time) & (ego_df_ini['simTime'] <= max_time)]
    speed = ego_df['v'].tolist()
    if (any(speed) == 0):
        return {"noStop_LST": -1}
    else:
        return {"noStop_LST": 1}


def launchTimeinTrafficLight_LST(data):
    '''
    待修改：
    红灯的状态值：1
    绿灯的状态值：0
    '''
    ego_df = data.ego_data
    simtime_of_redlight = ego_df[ego_df['stateMask'] == 0]['simTime']
    simtime_of_stop = ego_df[ego_df['v'] == 0]['simTime']
    if len(simtime_of_stop) or len(simtime_of_redlight):
        return {"timeInterval_LST": -1}
    simtime_of_launch = simtime_of_redlight[simtime_of_redlight.isin(simtime_of_stop)].tolist()
    if len(simtime_of_launch) == 0:
        return {"timeInterval_LST": -1}
    return {"timeInterval_LST": simtime_of_launch[-1] - simtime_of_launch[0]}


def crossJunctionToTargetLane_LST(data):
    ego_df = data.ego_data
    lane_in_leftturn = set(ego_df['lane_id'].tolist())
    scenario_name = find_nested_name(data.function_config["function"])
    target_lane_id = data.function_config["function"][scenario_name]["crossJunctionToTargetLane_LST"]['max']
    if target_lane_id not in lane_in_leftturn:
        return {"crossJunctionToTargetLane_LST": -1}
    else:
        return {"crossJunctionToTargetLane_LST": target_lane_id}


def keepInLane_LST(data):
    ego_df = data.ego_data
    notkeepinlane = ego_df[ego_df['laneOffset'] > ego_df['lane_width']/2].tolist()
    if len(notkeepinlane):
        return {"keepInLane_LST": -1}
    return {"keepInLane_LST": 1}


def leastLateralDistance_LST(data):
    ego_df = data.ego_data
    lane_width = ego_df[ego_df['x_relative_dist'] == 0]['lane_width']
    if lane_width.empty():
        return {"leastLateralDistance_LST": -1}
    else:
        y_relative_dist = ego_df[ego_df['x_relative_dist'] == 0]['y_relative_dist']
        if (y_relative_dist <= lane_width / 2).all():
            return {"leastLateralDistance_LST": 1}
        else:
            return {"leastLateralDistance_LST": -1}


def waitTimeAtCrosswalkwithPedestrian_LST(data):
    ego_df = data.ego_data
    object_df = data.object_data
    data['in_crosswalk'] = []
    position_data = data.drop_duplicates(subset=['cross_id', 'cross_coords'], inplace=True)
    for cross_id in position_data['cross_id'].tolist():
        for posX, posY in object_df['posX', 'posY']:
            pedestrian_pos = (posX, posY)
            plogan_pos = position_data[position_data['cross_id'] == cross_id]['cross_coords'].tolist()[0]
            if _is_pedestrian_in_crosswalk(plogan_pos, pedestrian_pos):
                data[data['playerId'] == 2]['in_crosswalk'] = 1
            else:
                data[data['playerId'] == 2]['in_crosswalk'] = 0
    car_stop_time = ego_df[ego_df['v'] == 0]['simTime']
    pedestrian_in_crosswalk_time = data[(data['in_crosswalk'] == 1)]['simTime']
    car_wait_pedestrian = car_stop_time.isin(pedestrian_in_crosswalk_time).tolist()
    return {'waitTimeAtCrosswalkwithPedestrian_LST': car_wait_pedestrian[-1] - car_wait_pedestrian[0] if len(
        car_wait_pedestrian) > 0 else 0}


def launchTimewhenPedestrianLeave_LST(data):
    ego_df = data.ego_data
    car_stop_time = ego_df[ego_df['v'] == 0]["simTime"]
    if car_stop_time.empty():
        return {"launchTimewhenPedestrianLeave_LST": -1}
    else:
        lane_width = ego_df[ego_df['v'] == 0]['lane_width'].tolist()[0]
        car_to_pedestrain = ego_df[ego_df['y_relative_dist'] <= lane_width / 2]["simTime"]
        legal_stop_time = car_stop_time.isin(car_to_pedestrain).tolist()
        return {"launchTimewhenPedestrianLeave_LST": legal_stop_time[-1] - legal_stop_time[0]}


def noCollision_LST(data):
    ego_df = data.ego_data
    if ego_df['relative_dist'].any() == 0:
        return {"noCollision_LST": -1}
    else:
        return {"noCollision_LST": 1}


def noReverse_LST(data):
    ego_df = data.ego_data
    if (ego_df["lon_v_vehicle"] * ego_df["posH"]).any() < 0:
        return {"noReverse_LST": -1}
    else:
        return {"noReverse_LST": 1}


def turnAround_LST(data):
    ego_df = data.ego_data
    if (ego_df["lon_v_vehicle"].tolist()[0] * ego_df["lon_v_vehicle"].tolist()[-1] < 0) and (
            ego_df["lon_v_vehicle"] * ego_df["posH"].all() > 0):
        return {"turnAround_LST": 1}
    else:
        return {"turnAround_LST": -1}


def laneOffset_LST(data):
    car_width = data.function_config['vehicle']['CAR_WIDTH']
    ego_df_ini = data.ego_data
    min_time = ego_df_ini['simTime'].min() + 5
    max_time = ego_df_ini['simTime'].max() - 5
    ego_df = ego_df_ini[(ego_df_ini['simTime'] >= min_time) & (ego_df_ini['simTime'] <= max_time)]
    laneoffset = ego_df['y_relative_dist'] - car_width / 2
    return {"laneOffset_LST": max(laneoffset)}


def maxLongitudeDist_LST(data):
    ego_df = data.ego_data
    longitude_dist = abs(ego_df[ego_df['v'] == 0]['x_relative_dist'].tolist())
    data.longitude_dist = min(abs(ego_df[ego_df['v'] == 0]['x_relative_dist'].tolist()))
    stop_time = ego_df[abs(ego_df['x_relative_dist']) == min(longitude_dist)]['simTime'].tolist()
    data.stop_time = min(stop_time)
    if len(longitude_dist) == 0:
        return {"maxLongitudeDist_LST": -1}
    generate_function_chart_data(data, 'maxLongitudeDist_LST')
    return {"maxLongDist_LST": min(longitude_dist)}


def noEmergencyBraking_LST(data):
    ego_df = data.ego_data
    ego_df['ip_dec'] = ego_df['v'].apply(
        get_interpolation, point1=[18, -5], point2=[72, -3.5])
    ego_df['slam_brake'] = (ego_df['accleX'] - ego_df['ip_dec']).apply(
        lambda x: 1 if x < 0 else 0)
    if sum(ego_df['slam_brake']) == 0:
        return {"noEmergencyBraking_LST": 1}
    else:
        return {"noEmergencyBraking_LST": -1}


def rightWarningSignal_PGVIL(data_processed) -> dict:
    """判断是否发出正确预警信号"""

    ego_df = data_processed.ego_data
    scenario_name = find_nested_name(data_processed.function_config["function"])
    correctwarning = scenario_sign_dict[scenario_name]

    if correctwarning is None:
        print("无法获取正确的预警信号标志位！")
        return None
    # 找出本行 correctwarning 和 ifwarning 相等，且 correctwarning 不是 NaN 的行
    warning_rows = ego_df[
        (ego_df["ifwarning"] == correctwarning) & (ego_df["ifwarning"].notna())
        ]

    if warning_rows.empty:
        return {"rightWarningSignal_PGVIL": -1}
    else:
        return {"rightWarningSignal_PGVIL": 1}


def latestWarningDistance_PGVIL(data_processed) -> dict:
    """预警距离计算流水线"""
    ego_df = data_processed.ego_data
    obj_df = data_processed.object_df
    warning_data = get_first_warning(data_processed)
    if warning_data is None:
        return {"latestWarningDistance_PGVIL": 0.0}

    ego, obj = extract_ego_obj(warning_data)
    distances = calculate_distance_PGVIL(
        np.array([[ego["posX"], ego["posY"]]]), obj[["posX", "posY"]].values
    )

    # 将计算结果保存到data对象中，供图表生成使用
    data_processed.warning_dist = distances
    if distances.size == 0:
        print("没有找到数据！")
        return {"latestWarningDistance_PGVIL": 15}  # 或返回其他默认值，如0.0

    return {"latestWarningDistance_PGVIL": float(np.min(distances))}


def latestWarningDistance_TTC_PGVIL(data_processed) -> dict:
    """TTC计算流水线"""
    ego_df = data_processed.ego_data
    obj_df = data_processed.object_df

    warning_data = get_first_warning(data_processed)
    if warning_data is None:
        return {"latestWarningDistance_TTC_PGVIL": 0.0}

    ego, obj = extract_ego_obj(warning_data)
    # 向量化计算
    ego_pos = np.array([[ego["posX"], ego["posY"]]])
    ego_speed = np.array([[ego["speedX"], ego["speedY"]]])
    obj_pos = obj[["posX", "posY"]].values
    obj_speed = obj[["speedX", "speedY"]].values

    distances = calculate_distance_PGVIL(ego_pos, obj_pos)
    rel_speeds = calculate_relative_speed_PGVIL(ego_speed, obj_speed)

    data_processed.warning_dist = distances
    data_processed.warning_speed = rel_speeds

    with np.errstate(divide="ignore", invalid="ignore"):
        ttc = np.where(rel_speeds != 0, distances / rel_speeds, np.inf)
    if ttc.size == 0:
        print("没有找到数据！")
        return {"latestWarningDistance_TTC_PGVIL": 2}  # 或返回其他默认值，如0.0
    data_processed.ttc = ttc

    return {"latestWarningDistance_TTC_PGVIL": float(np.nanmin(ttc))}


def earliestWarningDistance_PGVIL(data_processed) -> dict:
    """预警距离计算流水线"""
    ego_df = data_processed.ego_data
    obj_df = data_processed.object_df

    warning_data = get_first_warning(data_processed)
    if warning_data is None:
        return {"earliestWarningDistance_PGVIL": 0}

    ego, obj = extract_ego_obj(warning_data)
    distances = calculate_distance_PGVIL(
        np.array([[ego["posX"], ego["posY"]]]), obj[["posX", "posY"]].values
    )
    # 将计算结果保存到data对象中，供图表生成使用
    data_processed.warning_dist = distances
    if distances.size == 0:
        print("没有找到数据！")
        return {"earliestWarningDistance_PGVIL": 15}  # 或返回其他默认值，如0.0

    return {"earliestWarningDistance": float(np.min(distances))}


def earliestWarningDistance_TTC_PGVIL(data_processed) -> dict:
    """TTC计算流水线"""
    ego_df = data_processed.ego_data
    obj_df = data_processed.object_df

    warning_data = get_first_warning(data_processed)
    if warning_data is None:
        return {"earliestWarningDistance_TTC_PGVIL": 0.0}

    ego, obj = extract_ego_obj(warning_data)

    # 向量化计算
    ego_pos = np.array([[ego["posX"], ego["posY"]]])
    ego_speed = np.array([[ego["speedX"], ego["speedY"]]])
    obj_pos = obj[["posX", "posY"]].values
    obj_speed = obj[["speedX", "speedY"]].values

    distances = calculate_distance_PGVIL(ego_pos, obj_pos)
    rel_speeds = calculate_relative_speed_PGVIL(ego_speed, obj_speed)

    data_processed.warning_dist = distances
    data_processed.warning_speed = rel_speeds

    with np.errstate(divide="ignore", invalid="ignore"):
        ttc = np.where(rel_speeds != 0, distances / rel_speeds, np.inf)
    if ttc.size == 0:
        print("没有找到数据！")
        return {"earliestWarningDistance_TTC_PGVIL": 2}  # 或返回其他默认值，如0.0
    data_processed.ttc = ttc
    return {"earliestWarningDistance_TTC_PGVIL": float(np.nanmin(ttc))}


# def delayOfEmergencyBrakeWarning(data_processed) -> dict:
#     #预警时机相对背景车辆减速度达到-4m/s2后的时延
#     ego_df = data_processed.ego_data
#     obj_df = data_processed.object_df
#     warning_data = get_first_warning(data_processed)
#     if warning_data is None:
#         return {"delayOfEmergencyBrakeWarning": -1}
#     try:
#         ego, obj = extract_ego_obj(warning_data)
#         # 向量化计算
#         obj_speed = np.array([[obj_df["speedX"], obj_df["speedY"]]])
#         # 计算背景车辆减速度
#         simtime_gap = obj["simTime"].iloc[1] - obj["simTime"].iloc[0]
#         simtime_freq = 1 / simtime_gap#每秒采样频率
#         # simtime_freq为一个时间窗，找出时间窗内的最大减速度
#         obj_speed_magnitude = np.linalg.norm(obj_speed, axis=1)#速度向量的模长
#         obj_speed_change = np.diff(speed_magnitude)#速度模长的变化量
#         obj_deceleration = np.diff(obj_speed_magnitude) / simtime_gap
#         #找到最大减速度，若最大减速度小于-4m/s2，则计算最大减速度对应的时间，和warning_data的差值进行对比
#         max_deceleration = np.max(obj_deceleration)
#         if max_deceleration < -4:
#             max_deceleration_times = obj["simTime"].iloc[np.argmax(obj_deceleration)]
#             max_deceleration_time = max_deceleration_times.iloc[0]
#             delay_time = ego["simTime"] - max_deceleration_time
#             return {"delayOfEmergencyBrakeWarning": float(delay_time)}

#         else:
#             print("没有达到预警减速度阈值:-4m/s^2")
#             return {"delayOfEmergencyBrakeWarning": -1}


def warningDelayTime_PGVIL(data_processed) -> dict:
    """车端接收到预警到HMI发出预警的时延"""
    ego_df = data_processed.ego_data
    # #打印ego_df的列名
    # print(ego_df.columns.tolist())

    warning_data = get_first_warning(data_processed)

    if warning_data is None:
        return {"warningDelayTime_PGVIL": -1}
    try:
        ego, obj = extract_ego_obj(warning_data)

        # 找到event_Type不为空，且playerID为1的行
        rosbag_warning_rows = ego_df[(ego_df["event_Type"].notna())]

        first_time = rosbag_warning_rows["simTime"].iloc[0]
        warning_time = warning_data[warning_data["playerId"] == 1]["simTime"].iloc[0]
        delay_time = warning_time - first_time

        return {"warningDelayTime_PGVIL": float(delay_time)}

    except Exception as e:
        print(f"计算预警时延时发生错误: {e}")
        return {"warningDelayTime_PGVIL": -1}


def get_car_to_stop_line_distance(ego, car_point, stop_line_points):
    """
    计算主车后轴中心点到停止线的距离
    :return 距离
    """
    distance_carpoint_carhead = ego["dimX"] / 2 + ego["offX"]
    # 计算停止线的方向向量
    line_vector = np.array(
        [
            stop_line_points[1][0] - stop_line_points[0][0],
            stop_line_points[1][1] - stop_line_points[0][1],
        ]
    )
    direction_vector_norm = np.linalg.norm(line_vector)
    direction_vector_unit = (
        line_vector / direction_vector_norm
        if direction_vector_norm != 0
        else np.array([0, 0])
    )
    # 计算主车后轴中心点到停止线投影的坐标（垂足）
    projection_length = np.dot(car_point - stop_line_points[0], direction_vector_unit)
    perpendicular_foot = stop_line_points[0] + projection_length * direction_vector_unit

    # 计算主车后轴中心点到垂足的距离
    distance_to_foot = np.linalg.norm(car_point - perpendicular_foot)
    carhead_distance_to_foot = distance_to_foot - distance_carpoint_carhead

    return carhead_distance_to_foot


def ifCrossingRedLight_PGVIL(data_processed) -> dict:
    # 判断车辆是否闯红灯

    stop_line_points = np.array([(276.555, -35.575), (279.751, -33.683)])
    X_OFFSET = 258109.4239876
    Y_OFFSET = 4149969.964821
    stop_line_points += np.array([[X_OFFSET, Y_OFFSET]])
    ego_df = data_processed.ego_data

    prev_distance = float("inf")  # 初始化为正无穷大
    """
    traffic_light_status
    0x100000为绿灯，1048576
    0x1000000为黄灯，16777216
    0x10000000为红灯，268435456
    """
    red_light_violation = False
    for index, ego in ego_df.iterrows():
        car_point = (ego["posX"], ego["posY"])
        stateMask = ego["stateMask"]
        simTime = ego["simTime"]
        distance_to_stopline = get_car_to_stop_line_distance(
            ego, car_point, stop_line_points
        )

        # 主车车头跨越停止线时非绿灯，返回-1，闯红灯
        if prev_distance > 0 and distance_to_stopline < 0:
            if stateMask is not None and stateMask != 1048576:
                red_light_violation = True
            break
        prev_distance = distance_to_stopline

    if red_light_violation:
        return {"ifCrossingRedLight_PGVIL": -1}  # 闯红灯
    else:
        return {"ifCrossingRedLight_PGVIL": 1}  # 没有闯红灯


# def ifStopgreenWaveSpeedGuidance(data_processed) -> dict:
#     #在绿波车速引导期间是否发生停车


# def mindisStopline(data_processed) -> dict:
#     """
#     当有停车让行标志/标线时车辆最前端与停车让行线的最小距离应在0-4m之间
#     """
#     ego_df = data_processed.ego_data
#     obj_df = data_processed.object_df
#     stop_giveway_simtime = ego_df[
#         ego_df["sign_type1"] == 32 |
#         ego_df["stopline_type"] == 3
#     ]["simTime"]
#     stop_giveway_data = ego_df[
#         ego_df["sign_type1"] == 32 |
#         ego_df["stopline_type"] == 3
#     ]["simTime"]
#     if stop_giveway_simtime.empty:
#         print("没有停车让行标志/标线")

#     ego_data = stop_giveway_data[stop_giveway_data['playerId'] == 1]
#     distance_carpoint_carhead = ego_data['dimX'].iloc[0]/2 + ego_data['offX'].iloc[0]
#     distance_to_stoplines = []
#     for _,row in ego_data.iterrows():
#         ego_pos = np.array([row["posX"], row["posY"], row["posH"]])
#         stop_line_points = [
#             [row["stopline_x1"], row["stopline_y1"]],
#             [row["stopline_x2"], row["stopline_y2"]],
#         ]
#         distance_to_stopline = get_car_to_stop_line_distance(ego_pos, stop_line_points)
#         distance_to_stoplines.append(distance_to_stopline)

#     mindisStopline = np.min(distance_to_stoplines) - distance_carpoint_carhead
#     return {"mindisStopline": mindisStopline}


class FunctionRegistry:
    """动态函数注册器（支持参数验证）"""

    def __init__(self, data_processed):
        self.logger = LogManager().get_logger()  # 获取全局日志实例
        self.data = data_processed
        self.fun_config = data_processed.function_config["function"]
        self.level_3_merics = self._extract_level_3_metrics(self.fun_config)
        self._registry: Dict[str, Callable] = {}
        self._registry = self._build_registry()

    def _extract_level_3_metrics(self, config_node: dict) -> list:
        """DFS遍历提取第三层指标（时间复杂度O(n)）[4](@ref)"""
        metrics = []

        def _recurse(node):
            if isinstance(node, dict):
                if "name" in node and not any(
                        isinstance(v, dict) for v in node.values()
                ):
                    metrics.append(node["name"])
                for v in node.values():
                    _recurse(v)

        _recurse(config_node)
        self.logger.info(f"评比的功能指标列表:{metrics}")
        return metrics

    def _build_registry(self) -> dict:
        """自动注册指标函数（防御性编程）"""
        registry = {}
        for func_name in self.level_3_merics:
            try:
                registry[func_name] = globals()[func_name]
            except KeyError:
                print(f"未实现指标函数: {func_name}")
                self.logger.error(f"未实现指标函数: {func_name}")
        return registry

    def batch_execute(self) -> dict:
        """批量执行指标计算（带熔断机制）"""
        results = {}
        for name, func in self._registry.items():
            try:
                result = func(self.data)  # 统一传递数据上下文
                results.update(result)
            except Exception as e:
                print(f"{name} 执行失败: {str(e)}")
                self.logger.error(f"{name} 执行失败: {str(e)}", exc_info=True)
                results[name] = None
        self.logger.info(f"功能指标计算结果:{results}")
        return results


class FunctionManager:
    """管理功能指标计算的类"""

    def __init__(self, data_processed):
        self.data = data_processed
        self.function = FunctionRegistry(self.data)

    def report_statistic(self):
        """
        计算并报告功能指标结果。
        :return: 评估结果
        """
        function_result = self.function.batch_execute()

        print("\n[功能性表现及评价结果]")
        return function_result
        # self.logger.info(f'Function Result: {function_result}')


# 使用示例
if __name__ == "__main__":
    pass
    # print("\n[功能类表现及得分情况]")