zhaoyuan/modules/metric/safety.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
安全指标计算模块
"""

import numpy as np
import pandas as pd
import math
from collections import defaultdict
from typing import Dict, Any, List, Optional

from modules.lib.score import Score
from modules.lib.log_manager import LogManager

# 安全指标相关常量
SAFETY_INFO = [
    "simTime",
    "simFrame",
    "playerId",
    "posX",
    "posY",
    "posH",
    "speedX",
    "speedY",
    "accelX",
    "accelY",
    "v",
    "type"
]

# ----------------------
# 独立指标计算函数
# ----------------------
def calculate_ttc(data_processed) -> dict:
    """计算TTC (Time To Collision)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TTC": None}
    try:
        safety = SafetyCalculator(data_processed)
        ttc_value = safety.get_ttc_value()
        LogManager().get_logger().info(f"安全指标[TTC]计算结果: {ttc_value}")
        return {"TTC": ttc_value}
    except Exception as e:
        LogManager().get_logger().error(f"TTC计算异常: {str(e)}", exc_info=True)
        return {"TTC": None}

def calculate_mttc(data_processed) -> dict:
    """计算MTTC (Modified Time To Collision)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"MTTC": None}
    try:
        safety = SafetyCalculator(data_processed)
        mttc_value = safety.get_mttc_value()
        LogManager().get_logger().info(f"安全指标[MTTC]计算结果: {mttc_value}")
        return {"MTTC": mttc_value}
    except Exception as e:
        LogManager().get_logger().error(f"MTTC计算异常: {str(e)}", exc_info=True)
        return {"MTTC": None}

def calculate_thw(data_processed) -> dict:
    """计算THW (Time Headway)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"THW": None}
    try:
        safety = SafetyCalculator(data_processed)
        thw_value = safety.get_thw_value()
        LogManager().get_logger().info(f"安全指标[THW]计算结果: {thw_value}")
        return {"THW": thw_value}
    except Exception as e:
        LogManager().get_logger().error(f"THW计算异常: {str(e)}", exc_info=True)
        return {"THW": None}

def calculate_tlc(data_processed) -> dict:
    """计算TLC (Time to Line Crossing)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TLC": None}
    try:
        safety = SafetyCalculator(data_processed)
        tlc_value = safety.get_tlc_value()
        LogManager().get_logger().info(f"安全指标[TLC]计算结果: {tlc_value}")
        return {"TLC": tlc_value}
    except Exception as e:
        LogManager().get_logger().error(f"TLC计算异常: {str(e)}", exc_info=True)
        return {"TLC": None}

def calculate_ttb(data_processed) -> dict:
    """计算TTB (Time to Brake)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TTB": None}
    try:
        safety = SafetyCalculator(data_processed)
        ttb_value = safety.get_ttb_value()
        LogManager().get_logger().info(f"安全指标[TTB]计算结果: {ttb_value}")
        return {"TTB": ttb_value}
    except Exception as e:
        LogManager().get_logger().error(f"TTB计算异常: {str(e)}", exc_info=True)
        return {"TTB": None}

def calculate_tm(data_processed) -> dict:
    """计算TM (Time Margin)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"TM": None}
    try:
        safety = SafetyCalculator(data_processed)
        tm_value = safety.get_tm_value()
        LogManager().get_logger().info(f"安全指标[TM]计算结果: {tm_value}")
        return {"TM": tm_value}
    except Exception as e:
        LogManager().get_logger().error(f"TM计算异常: {str(e)}", exc_info=True)
        return {"TM": None}

# def calculate_MPrTTC(data_processed) -> dict:
#     """计算MPrTTC (Model Predictive Time-to-Collision)"""
#     if data_processed is None or not hasattr(data_processed, 'object_df'):
#         return {"MPrTTC": None}
#     try:
#         safety = SafetyCalculator(data_processed)
#         mprttc_value = safety.get_mprttc_value()
#         LogManager().get_logger().info(f"安全指标[MPrTTC]计算结果: {mprttc_value}")
#         return {"MPrTTC": mprttc_value}
#     except Exception as e:
#         LogManager().get_logger().error(f"MPrTTC计算异常: {str(e)}", exc_info=True)
#         return {"MPrTTC": None}

# def calculate_pet(data_processed) -> dict:
#     # PET (Post Encroachment Time)
#     if data_processed is None or not hasattr(data_processed, 'object_df'):
#         return {"PET": None}
#     try:
#         safety = SafetyCalculator(data_processed)
#         pet_value = safety.get_pet_value()
#         LogManager().get_logger().info(f"安全指标[PET]计算结果: {pet_value}")
#         return {"PET": pet_value}
#     except Exception as e:
#         LogManager().get_logger().error(f"PET计算异常: {str(e)}", exc_info=True)
#         return {"PET": None}

def calculate_dtc(data_processed) -> dict:
    """计算DTC (Distance to Collision)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"DTC": None}
    try:
        safety = SafetyCalculator(data_processed)
        dtc_value = safety.get_dtc_value()
        LogManager().get_logger().info(f"安全指标[DTC]计算结果: {dtc_value}")
        return {"DTC": dtc_value}
    except Exception as e:
        LogManager().get_logger().error(f"DTC计算异常: {str(e)}", exc_info=True)
        return {"DTC": None}

def calculate_dtc(data_processed) -> dict:
    """计算DTC (Distance to Collision)"""
    if data_processed is None or not hasattr(data_processed, 'object_df'):
        return {"DTC": None}
    try:
        safety = SafetyCalculator(data_processed)
        dtc_value = safety.get_dtc_value()
        LogManager().get_logger().info(f"安全指标[DTC]计算结果: {dtc_value}")
        return {"DTC": dtc_value}
    except Exception as e:
        LogManager().get_logger().error(f"DTC计算异常: {str(e)}", exc_info=True)
        return {"DTC": None}

def calculate_collisionrisk(data_processed) -> dict:
    """计算碰撞风险"""
    safety = SafetyCalculator(data_processed)
    collision_risk_value = safety.get_collision_risk_value()
    LogManager().get_logger().info(f"安全指标[collisionRisk]计算结果: {collision_risk_value}")
    return {"collisionRisk": collision_risk_value}

def calculate_lonsd(data_processed) -> dict:
    """计算纵向安全距离"""
    safety = SafetyCalculator(data_processed)
    lonsd_value = safety.get_lonsd_value()
    LogManager().get_logger().info(f"安全指标[LonSD]计算结果: {lonsd_value}")
    return {"LonSD": lonsd_value}

def calculate_latsd(data_processed) -> dict:
    """计算横向安全距离"""
    safety = SafetyCalculator(data_processed)
    latsd_value = safety.get_latsd_value()
    LogManager().get_logger().info(f"安全指标[LatSD]计算结果: {latsd_value}")
    return {"LatSD": latsd_value}

def calculate_btn(data_processed) -> dict:
    """计算制动威胁数"""
    safety = SafetyCalculator(data_processed)
    btn_value = safety.get_btn_value()
    LogManager().get_logger().info(f"安全指标[BTN]计算结果: {btn_value}")
    return {"BTN": btn_value}

def calculate_collisionseverity(data_processed) -> dict:
    """计算碰撞严重性"""
    safety = SafetyCalculator(data_processed)
    collision_severity_value = safety.get_collision_severity_value()
    LogManager().get_logger().info(f"安全指标[collisionSeverity]计算结果: {collision_severity_value}")
    return {"collisionSeverity": collision_severity_value}


class SafetyRegistry:
    """安全指标注册器"""
    
    def __init__(self, data_processed):
        self.logger = LogManager().get_logger()
        self.data = data_processed
        self.safety_config = data_processed.safety_config["safety"]
        self.metrics = self._extract_metrics(self.safety_config)
        self._registry = self._build_registry()
    
    def _extract_metrics(self, config_node: dict) -> list:
        """从配置中提取指标名称"""
        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 metric_name in self.metrics:
            func_name = f"calculate_{metric_name.lower()}"
            if func_name in globals():
                registry[metric_name] = globals()[func_name]
            else:
                self.logger.warning(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:
                self.logger.error(f"{name} 执行失败: {str(e)}", exc_info=True)
                results[name] = None
        self.logger.info(f'安全指标计算结果:{results}')
        return results


class SafeManager:
    """安全指标管理类"""
    
    def __init__(self, data_processed):
        self.data = data_processed
        self.registry = SafetyRegistry(self.data)
    
    def report_statistic(self):
        """计算并报告安全指标结果"""
        safety_result = self.registry.batch_execute()
        
        return safety_result
    

class SafetyCalculator:
    """安全指标计算类 - 兼容Safe类风格"""

    def __init__(self, data_processed):
        self.logger = LogManager().get_logger()
        self.data_processed = data_processed

        self.df = data_processed.object_df.copy()
        self.ego_df = data_processed.ego_data.copy()  # 使用copy()避免修改原始数据
        self.obj_id_list = data_processed.obj_id_list

        self.metric_list = [
            'TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'TM', 'DTC', 'LonSD', 'LatSD', 'BTN', 'collisionRisk', 'collisionSeverity'
        ]

        # 初始化默认值
        self.calculated_value = {
            "TTC": 10.0,
            "MTTC": 10.0,
            "THW": 10.0,
            "TLC": 10.0,
            "TTB": 10.0,
            "TM": 10.0,
            # "MPrTTC": 10.0,
            # "PET": 10.0,
            "DTC": 10.0,
            "LatSD": 3.0,
            "BTN": 1.0,
            "collisionRisk": 0.0,
            "collisionSeverity": 0.0,
        }

        self.time_list = self.ego_df['simTime'].values.tolist()
        self.frame_list = self.ego_df['simFrame'].values.tolist()
        self.collisionRisk = 0
        self.empty_flag = True

        self.logger.info("SafetyCalculator初始化完成，场景中包含自车的目标物一共为: %d", len(self.obj_id_list))

        if len(self.obj_id_list) > 1:
            self.unsafe_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_time_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_dist_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_acce_drac_df = pd.DataFrame(
                columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_acce_xtn_df = pd.DataFrame(
                columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.unsafe_prob_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
            self.most_dangerous = {}
            self.pass_percent = {}
            self.logger.info("开始执行安全参数计算 _safe_param_cal_new")
            self._safe_param_cal_new()
            self.logger.info("安全参数计算完成")

    def _safe_param_cal_new(self):
        self.logger.debug("进入 _safe_param_cal_new 方法")
        # 直接复用Safe类的实现
        Tc = 0.3  # 安全距离

        rho = self.data_processed.vehicle_config["RHO"]
        ego_accel_max = self.data_processed.vehicle_config["EGO_ACCEL_MAX"]
        obj_decel_max = self.data_processed.vehicle_config["OBJ_DECEL_MAX"]
        ego_decel_min = self.data_processed.vehicle_config["EGO_DECEL_MIN"]
        ego_decel_lon_max = self.data_processed.vehicle_config["EGO_DECEL_LON_MAX"]
        ego_decel_lat_max = self.data_processed.vehicle_config["EGO_DECEL_LAT_MAX"]
        driver_reaction_time = self.data_processed.vehicle_config["RHO"]
        ego_decel_max = np.sqrt(ego_decel_lon_max ** 2 + ego_decel_lat_max ** 2)
        x_relative_start_dist = self.ego_df["x_relative_start_dist"]

        obj_dict = defaultdict(dict)
        obj_data_dict = self.df.to_dict('records')
        for item in obj_data_dict:
            obj_dict[item['simFrame']][item['playerId']] = item

        df_list = []
        EGO_PLAYER_ID = 1

        for frame_num in self.frame_list:
            ego_data = obj_dict[frame_num][EGO_PLAYER_ID]
            v1 = ego_data['v']
            x1 = ego_data['posX']
            y1 = ego_data['posY']
            h1 = ego_data['posH']
            laneOffset = ego_data["laneOffset"]

            v_x1 = ego_data['speedX']
            v_y1 = ego_data['speedY']
            a_x1 = ego_data['accelX']
            a_y1 = ego_data['accelY']
            a1 = np.sqrt(a_x1 ** 2 + a_y1 ** 2)

            for playerId in self.obj_id_list:
                if playerId == EGO_PLAYER_ID:
                    continue
                try:
                    obj_data = obj_dict[frame_num][playerId]
                except KeyError:
                    continue

                x2 = obj_data['posX']
                y2 = obj_data['posY']
                dist = self.dist(x1, y1, x2, y2)
                obj_data['dist'] = dist

                v_x2 = obj_data['speedX']
                v_y2 = obj_data['speedY']
                v2 = obj_data['v']
                a_x2 = obj_data['accelX']
                a_y2 = obj_data['accelY']
                a2 = np.sqrt(a_x2 ** 2 + a_y2 ** 2)

                dx, dy = x2 - x1, y2 - y1
                
                # 计算目标车相对于自车的方位角
                beta = math.atan2(dy, dx)
                
                # 将全局坐标系下的相对位置向量转换到自车坐标系
                # 自车坐标系：车头方向为x轴正方向，车辆左侧为y轴正方向
                h1_rad = math.radians(90 - h1)  # 转换为与x轴的夹角
                
                # 坐标变换
                lon_d = dx * math.cos(h1_rad) + dy * math.sin(h1_rad)  # 纵向距离（前为正，后为负）
                lat_d = abs(-dx * math.sin(h1_rad) + dy * math.cos(h1_rad))  # 横向距离（取绝对值）
                
                obj_dict[frame_num][playerId]['lon_d'] = lon_d
                obj_dict[frame_num][playerId]['lat_d'] = lat_d

                if lon_d > 100 or lon_d < -5 or lat_d > 4:
                    continue

                self.empty_flag = False

                vx, vy = v_x1 - v_x2, v_y1 - v_y2
                ax, ay = a_x2 - a_x1, a_y2 - a_y1
                relative_v = np.sqrt(vx ** 2 + vy ** 2)

                v_ego_p = self._cal_v_ego_projection(dx, dy, v_x1, v_y1)
                v_obj_p = self._cal_v_ego_projection(dx, dy, v_x2, v_y2)
                vrel_projection_in_dist = self._cal_v_projection(dx, dy, vx, vy)
                arel_projection_in_dist = self._cal_a_projection(dx, dy, vx, vy, ax, ay, x1, y1, x2, y2, v_x1, v_y1,
                                                                 v_x2, v_y2)

                obj_dict[frame_num][playerId]['vrel_projection_in_dist'] = vrel_projection_in_dist
                obj_dict[frame_num][playerId]['arel_projection_in_dist'] = arel_projection_in_dist
                obj_dict[frame_num][playerId]['v_ego_projection_in_dist'] = v_ego_p
                obj_dict[frame_num][playerId]['v_obj_projection_in_dist'] = v_obj_p

                obj_type = obj_data['type']

                TTC = self._cal_TTC(dist, vrel_projection_in_dist) if abs(vrel_projection_in_dist) > 1e-6 else None
                MTTC = self._cal_MTTC(dist, vrel_projection_in_dist, arel_projection_in_dist)
                THW = self._cal_THW(dist, v_ego_p) if abs(v_ego_p) > 1e-6 else None
                TLC = self._cal_TLC(v1, h1, laneOffset)
                TTB = self._cal_TTB(x_relative_start_dist, relative_v, ego_decel_max)
                TM = self._cal_TM(x_relative_start_dist, v2, a2, v1, a1)
                DTC = self._cal_DTC(vrel_projection_in_dist, arel_projection_in_dist, driver_reaction_time)
                # MPrTTC = self._cal_MPrTTC(x_relative_start_dist)
                # PET = self._cal_PET(x_relative_start_dist, v2, a2, v1, a1)


                LonSD = self._cal_longitudinal_safe_dist(v_ego_p, v_obj_p, rho, ego_accel_max, ego_decel_min, obj_decel_max)

                lat_dist = 0.5
                v_right = v1
                v_left = v2
                a_right_lat_brake_min = 1
                a_left_lat_brake_min = 1
                a_lat_max = 5

                LatSD = self._cal_lateral_safe_dist(lat_dist, v_right, v_left, rho, a_right_lat_brake_min,
                                                    a_left_lat_brake_min, a_lat_max)

                # 使用自车坐标系下的纵向加速度
                lon_a1 = a_x1 * math.cos(h1_rad) + a_y1 * math.sin(h1_rad)
                lon_a2 = a_x2 * math.cos(h1_rad) + a_y2 * math.sin(h1_rad)
                lon_a = abs(lon_a1 - lon_a2)
                lon_d = max(0.1, lon_d)  # 确保纵向距离为正
                lon_v = v_x1 * math.cos(h1_rad) + v_y1 * math.sin(h1_rad)
                BTN = self._cal_BTN_new(lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max)

                # 使用自车坐标系下的横向加速度
                lat_a1 = -a_x1 * math.sin(h1_rad) + a_y1 * math.cos(h1_rad)
                lat_a2 = -a_x2 * math.sin(h1_rad) + a_y2 * math.cos(h1_rad)
                lat_a = abs(lat_a1 - lat_a2)
                lat_v = -v_x1 * math.sin(h1_rad) + v_y1 * math.cos(h1_rad)

                obj_dict[frame_num][playerId]['lat_v_rel'] = lat_v - (-v_x2 * math.sin(h1_rad) + v_y2 * math.cos(h1_rad))
                obj_dict[frame_num][playerId]['lon_v_rel'] = lon_v - (v_x2 * math.cos(h1_rad) + v_y2 * math.sin(h1_rad))

                TTC = None if (TTC is None or TTC < 0) else TTC
                MTTC = None if (MTTC is None or MTTC < 0) else MTTC
                THW = None if (THW is None or THW < 0) else THW
                TLC = None if (TLC is None or TLC < 0) else TLC
                TTB = None if (TTB is None or TTB < 0) else TTB
                TM = None if (TM is None or TM < 0) else TM
                DTC = None if (DTC is None or DTC < 0) else DTC

                obj_dict[frame_num][playerId]['TTC'] = TTC
                obj_dict[frame_num][playerId]['MTTC'] = MTTC
                obj_dict[frame_num][playerId]['THW'] = THW
                obj_dict[frame_num][playerId]['TLC'] = TLC
                obj_dict[frame_num][playerId]['TTB'] = TTB
                obj_dict[frame_num][playerId]['TM'] = TM
                obj_dict[frame_num][playerId]['DTC'] = DTC
                obj_dict[frame_num][playerId]['LonSD'] = LonSD
                obj_dict[frame_num][playerId]['LatSD'] = LatSD
                obj_dict[frame_num][playerId]['BTN'] = abs(BTN)

                collisionSeverity = 0
                pr_death = 0
                collisionRisk = 0

                obj_dict[frame_num][playerId]['collisionSeverity'] = collisionSeverity * 100
                obj_dict[frame_num][playerId]['pr_death'] = pr_death * 100
                obj_dict[frame_num][playerId]['collisionRisk'] = collisionRisk * 100

            df_fnum = pd.DataFrame(obj_dict[frame_num].values())
            df_list.append(df_fnum)

        df_safe = pd.concat(df_list)
        col_list = ['simTime', 'simFrame', 'playerId',
                    'TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'TM', 'DTC', 'LonSD', 'LatSD', 'BTN',
                    'collisionSeverity', 'pr_death', 'collisionRisk']
        self.df_safe = df_safe[col_list].reset_index(drop=True)

    def _cal_v_ego_projection(self, dx, dy, v_x1, v_y1):
        # 计算 AB 连线的向量 AB
        # dx = x2 - x1
        # dy = y2 - y1

        # 计算 AB 连线的模长 |AB|
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)

        # 计算 AB 连线的单位向量 U_AB
        U_ABx = dx / AB_mod
        U_ABy = dy / AB_mod

        # 计算 A 在 AB 连线上的速度 V1_on_AB
        V1_on_AB = v_x1 * U_ABx + v_y1 * U_ABy

        return V1_on_AB

    def _cal_v_projection(self, dx, dy, vx, vy):
        # 计算 AB 连线的向量 AB
        # dx = x2 - x1
        # dy = y2 - y1

        # 计算 AB 连线的模长 |AB|
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)

        # 计算 AB 连线的单位向量 U_AB
        U_ABx = dx / AB_mod
        U_ABy = dy / AB_mod

        # 计算 A 相对于 B 的速度 V_relative
        # vx = vx1 - vx2
        # vy = vy1 - vy2

        # 计算 A 相对于 B 在 AB 连线上的速度 V_on_AB
        V_on_AB = vx * U_ABx + vy * U_ABy

        return V_on_AB

    def _cal_a_projection(self, dx, dy, vx, vy, ax, ay, x1, y1, x2, y2, v_x1, v_y1, v_x2, v_y2):
        # 计算 AB 连线的向量 AB
        # dx = x2 - x1
        # dy = y2 - y1

        # 计算 θ
        V_mod = math.sqrt(vx ** 2 + vy ** 2)
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)
        if V_mod == 0 or AB_mod == 0:
            return 0

        cos_theta = (vx * dx + vy * dy) / (V_mod * AB_mod)
        theta = math.acos(cos_theta)

        # 计算 AB 连线的模长 |AB|
        AB_mod = math.sqrt(dx ** 2 + dy ** 2)

        # 计算 AB 连线的单位向量 U_AB
        U_ABx = dx / AB_mod
        U_ABy = dy / AB_mod

        # 计算 A 相对于 B 的加速度 a_relative
        # ax = ax1 - ax2
        # ay = ay1 - ay2

        # 计算 A 相对于 B 在 AB 连线上的加速度 a_on_AB
        a_on_AB = ax * U_ABx + ay * U_ABy

        VA = np.array([v_x1, v_y1])
        VB = np.array([v_x2, v_y2])
        D_A = np.array([x1, y1])
        D_B = np.array([x2, y2])
        V_r = VA - VB
        V = np.linalg.norm(V_r)
        w = self._cal_relative_angular_v(theta, D_A, D_B, VA, VB)
        a_on_AB_back = self._calculate_derivative(a_on_AB, w, V, theta)
        return a_on_AB_back

    # 计算相对加速度
    def _calculate_derivative(self, a, w, V, theta):
        # 计算(V×cos(θ))'的值
        # derivative = a * math.cos(theta) - w * V * math.sin(theta)theta
        derivative = a - w * V * math.sin(theta)
        return derivative

    def _cal_relative_angular_v(self, theta, A, B, VA, VB):
        dx = A[0] - B[0]
        dy = A[1] - B[1]
        dvx = VA[0] - VB[0]
        dvy = VA[1] - VB[1]
        # (dx * dvy - dy * dvx)
        angular_velocity = math.sqrt(dvx ** 2 + dvy ** 2) * math.sin(theta) / math.sqrt(dx ** 2 + dy ** 2)
        return angular_velocity

    def _death_pr(self, obj_type, v_relative):
        if obj_type == 5:
            p_death = 1 / (1 + np.exp(7.723 - 0.15 * v_relative))
        else:
            p_death = 1 / (1 + np.exp(8.192 - 0.12 * v_relative))
        return p_death

    def _cal_collisionRisk_level(self, obj_type, v_relative, collisionSeverity):
        if obj_type == 5:
            p_death = 1 / (1 + np.exp(7.723 - 0.15 * v_relative))
        else:
            p_death = 1 / (1 + np.exp(8.192 - 0.12 * v_relative))
        collisionRisk = 0.4 * p_death + 0.6 * collisionSeverity
        return collisionRisk

    # 求两车之间当前距离
    def dist(self, x1, y1, x2, y2):
        dist = np.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
        return dist

    # TTC (time to collision)
    def _cal_TTC(self, dist, vrel_projection_in_dist):
        if vrel_projection_in_dist == 0:
            return math.inf
        TTC = dist / vrel_projection_in_dist
        return TTC

    def _cal_MTTC(self, dist, vrel_projection_in_dist, arel_projection_in_dist):
        MTTC = math.nan
        if arel_projection_in_dist != 0:
            tmp = vrel_projection_in_dist ** 2 + 2 * arel_projection_in_dist * dist
            if tmp < 0:
                return math.nan
            t1 = (-1 * vrel_projection_in_dist - math.sqrt(tmp)) / arel_projection_in_dist
            t2 = (-1 * vrel_projection_in_dist + math.sqrt(tmp)) / arel_projection_in_dist
            if t1 > 0 and t2 > 0:
                if t1 >= t2:
                    MTTC = t2
                elif t1 < t2:
                    MTTC = t1
            elif t1 > 0 and t2 <= 0:
                MTTC = t1
            elif t1 <= 0 and t2 > 0:
                MTTC = t2
        if arel_projection_in_dist == 0 and vrel_projection_in_dist > 0:
            MTTC = dist / vrel_projection_in_dist
        return MTTC

    # THW (time headway)
    def _cal_THW(self, dist, v_ego_projection_in_dist):
        if not v_ego_projection_in_dist:
            THW = None
        else:
            THW = dist / v_ego_projection_in_dist
        return THW
    # TLC (time to line crossing)
    def _cal_TLC(self, ego_v, ego_yaw, laneOffset):
        TLC = laneOffset/ego_v/np.sin(ego_yaw) if ((ego_v != 0) and (np.sin(ego_yaw) != 0)) else 10.0
        if TLC < 0:
            TLC = None
        return TLC

    def _cal_TTB(self, x_relative_start_dist, relative_v, ego_decel_max):
        if len(x_relative_start_dist):
            return None
        if (ego_decel_max  == 0) or (relative_v == 0):
            return self.calculated_value["TTB"]
        else:
            x_relative_start_dist0 = x_relative_start_dist.tolist()[0]
            TTB = (x_relative_start_dist0 + relative_v * relative_v/2/ego_decel_max)/relative_v
            return TTB

    def _cal_TM(self, x_relative_start_dist, v2, a2, v1, a1):
        if len(x_relative_start_dist):
            return None
        if (a2 == 0) or (v1 == 0):
            return self.calculated_value["TM"]
        if a1 == 0:
            return None
        x_relative_start_dist0 = x_relative_start_dist.tolist()[0]
        TM = (x_relative_start_dist0 + v2**2/(2*a2) - v1**2/(2*a1)) / v1
        return TM

    # def _cal_MPrTTC(self, T=5, c = False, collision_dist = 5.99):
    #     time_interval = self.ego_df['simTime'].tolist()[1] - self.ego_df['simTime'].tolist()[0]
    #
    #     for i in range(len(self.obj_id_list)):
    #         for j in range(T):
    #             MPrTTC = j * time_interval

    def _cal_DTC(self, v_on_dist, a_on_dist, t):
        if a_on_dist == 0:
            return None
        DTC = v_on_dist * t + v_on_dist ** 2 / a_on_dist
        return DTC


    def velocity(self, v_x, v_y):
        v = math.sqrt(v_x ** 2 + v_y ** 2) * 3.6
        return v

    def _cal_longitudinal_safe_dist(self, v_ego_p, v_obj_p, rho, ego_accel_max, ego_decel_min, ego_decel_max):
        lon_dist_min = v_ego_p * rho + ego_accel_max * (rho ** 2) / 2 + (v_ego_p + rho * ego_accel_max) ** 2 / (
                2 * ego_decel_min) - v_obj_p ** 2 / (2 * ego_decel_max)
        return lon_dist_min

    def _cal_lateral_safe_dist(self, lat_dist, v_right, v_left, rho, a_right_lat_brake_min,
                               a_left_lat_brake_min, a_lat_max):
        # 检查除数是否为零
        if a_right_lat_brake_min == 0 or a_left_lat_brake_min == 0:
            return self._default_value('LatSD')  # 返回默认值
            
        v_right_rho = v_right + rho * a_lat_max
        v_left_rho = v_left + rho * a_lat_max
        dist_min = lat_dist + (
            (v_right + v_right_rho) * rho / 2
            + v_right_rho**2 / a_right_lat_brake_min / 2
            + ((v_left + v_right_rho) * rho / 2)
            + v_left_rho**2 / a_left_lat_brake_min / 2
        )
        return dist_min

    # DRAC (decelerate required avoid collision)
    def _cal_DRAC(self, dist, vrel_projection_in_dist, len1, len2, width1, width2, o_x1, o_x2):
        dist_length = dist - (len2 / 2 - o_x2 + len1 / 2 + o_x1)  # 4.671
        if dist_length < 0:
            dist_width = dist - (width2 / 2 + width1 / 2)
            if dist_width < 0:
                return math.inf
            else:
                d = dist_width
        else:
            d = dist_length
        DRAC = vrel_projection_in_dist ** 2 / (2 * d)
        return DRAC

    # BTN (brake threat number)
    def _cal_BTN_new(self, lon_a1, lon_a, lon_d, lon_v, ego_decel_lon_max):
        BTN = (lon_a1 + lon_a - lon_v ** 2 / (2 * lon_d)) / ego_decel_lon_max  # max_ay为此车可实现的最大纵向加速度，目前为本次实例里的最大值
        return BTN

    # STN (steer threat number)
    def _cal_STN_new(self, ttc, lat_a1, lat_a, lat_d, lat_v, ego_decel_lat_max, width1, width2):
        STN = (lat_a1 + lat_a + 2 / ttc ** 2 * (lat_d + abs(ego_decel_lat_max * lat_v) * (
                width1 + width2) / 2 + abs(lat_v * ttc))) / ego_decel_lat_max
        return STN

    # BTN (brake threat number)
    def cal_BTN(self, a_y1, ay, dy, vy, max_ay):
        BTN = (a_y1 + ay - vy ** 2 / (2 * dy)) / max_ay  # max_ay为此车可实现的最大纵向加速度，目前为本次实例里的最大值
        return BTN

    # STN (steer threat number)
    def cal_STN(self, ttc, a_x1, ax, dx, vx, max_ax, width1, width2):
        STN = (a_x1 + ax + 2 / ttc ** 2 * (dx + np.sign(max_ax * vx) * (width1 + width2) / 2 + vx * ttc)) / max_ax
        return STN

    # 追尾碰撞风险
    def _normal_distribution(self, x):
        mean = 1.32
        std_dev = 0.26
        return (1 / (math.sqrt(std_dev * 2 * math.pi))) * math.exp(-0.5 * (x - mean) ** 2 / std_dev)

    def continuous_group(self, df):
        time_list = df['simTime'].values.tolist()
        frame_list = df['simFrame'].values.tolist()

        group_time = []
        group_frame = []
        sub_group_time = []
        sub_group_frame = []

        for i in range(len(frame_list)):
            if not sub_group_time or frame_list[i] - frame_list[i - 1] <= 1:
                sub_group_time.append(time_list[i])
                sub_group_frame.append(frame_list[i])
            else:
                group_time.append(sub_group_time)
                group_frame.append(sub_group_frame)
                sub_group_time = [time_list[i]]
                sub_group_frame = [frame_list[i]]

        group_time.append(sub_group_time)
        group_frame.append(sub_group_frame)
        group_time = [g for g in group_time if len(g) >= 2]
        group_frame = [g for g in group_frame if len(g) >= 2]

        # 输出图表值
        time = [[g[0], g[-1]] for g in group_time]
        frame = [[g[0], g[-1]] for g in group_frame]

        unfunc_time_df = pd.DataFrame(time, columns=['start_time', 'end_time'])
        unfunc_frame_df = pd.DataFrame(frame, columns=['start_frame', 'end_frame'])

        unfunc_df = pd.concat([unfunc_time_df, unfunc_frame_df], axis=1)
        return unfunc_df
        # 统计最危险的指标
    
    def _safe_statistic_most_dangerous(self):
        min_list = ['TTC', 'MTTC', 'THW', 'TLC', 'TTB', 'LonSD', 'LatSD', 'TM']
        max_list = ['DTC', 'BTN', 'collisionRisk', 'collisionSeverity']
        result = {}
        for metric in min_list:
            if metric in self.metric_list:
                if metric in self.df.columns:
                    val = self.df[metric].min()
                    result[metric] = float(val) if pd.notnull(val) else self._default_value(metric)
                else:
                    result[metric] = self._default_value(metric)
        for metric in max_list:
            if metric in self.metric_list:
                if metric in self.df.columns:
                    val = self.df[metric].max()
                    result[metric] = float(val) if pd.notnull(val) else self._default_value(metric)
                else:
                    result[metric] = self._default_value(metric)
        return result

    def _safe_no_obj_statistic(self):
        # 仅有自车时的默认值
        result = {metric: self._default_value(metric) for metric in self.metric_list}
        return result

    def _default_value(self, metric):
        # 统一默认值
        defaults = {
            'TTC': 10.0,
            'MTTC': 4.2,
            'THW': 2.1,
            'TLC': 10.0,
            'TTB': 10.0,
            'TM': 10.0,
            'DTC': 10.0,
            'LonSD': 10.0,
            'LatSD': 2.0,
            'BTN': 1.0,
            'collisionRisk': 0.0,
            'collisionSeverity': 0.0
        }
        return defaults.get(metric, None)

    def report_statistic(self):
        if len(self.obj_id_list) == 1:
            safety_result = self._safe_no_obj_statistic()
        else:
            safety_result = self._safe_statistic_most_dangerous()
        evaluator = Score(self.data_processed.safety_config)
        result = evaluator.evaluate(safety_result)
        print("\n[安全性表现及得分情况]")
        return result

    def get_ttc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TTC')
        ttc_values = self.df_safe['TTC'].dropna()
        return float(ttc_values.min()) if not ttc_values.empty else self._default_value('TTC')

    def get_mttc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('MTTC')
        mttc_values = self.df_safe['MTTC'].dropna()
        return float(mttc_values.min()) if not mttc_values.empty else self._default_value('MTTC')

    def get_thw_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('THW')
        thw_values = self.df_safe['THW'].dropna()
        return float(thw_values.min()) if not thw_values.empty else self._default_value('THW')

    def get_tlc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TLC')
        tlc_values = self.df_safe['TLC'].dropna()
        return float(tlc_values.min()) if not tlc_values.empty else self._default_value('TLC')

    def get_ttb_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TTB')
        ttb_values = self.df_safe['TTB'].dropna()
        return float(ttb_values.min()) if not ttb_values.empty else self._default_value('TTB')

    def get_tm_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('TM')
        tm_values = self.df_safe['TM'].dropna()
        return float(tm_values.min()) if not tm_values.empty else self._default_value('TM')

    def get_dtc_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('DTC')
        dtc_values = self.df_safe['DTC'].dropna()
        return float(dtc_values.min()) if not dtc_values.empty else self._default_value('DTC')

    def get_lonsd_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('LonSD')
        lonsd_values = self.df_safe['LonSD'].dropna()
        return float(lonsd_values.mean()) if not lonsd_values.empty else self._default_value('LonSD')

    def get_latsd_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('LatSD')
        latsd_values = self.df_safe['LatSD'].dropna()
        # 使用最小值而非平均值，与safety1.py保持一致
        return float(latsd_values.min()) if not latsd_values.empty else self._default_value('LatSD')

    def get_btn_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('BTN')
        btn_values = self.df_safe['BTN'].dropna()
        return float(btn_values.max()) if not btn_values.empty else self._default_value('BTN')

    def get_collision_risk_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('collisionRisk')
        risk_values = self.df_safe['collisionRisk'].dropna()
        return float(risk_values.max()) if not risk_values.empty else self._default_value('collisionRisk')

    def get_collision_severity_value(self) -> float:
        if self.empty_flag or self.df_safe is None:
            return self._default_value('collisionSeverity')
        severity_values = self.df_safe['collisionSeverity'].dropna()
        return float(severity_values.max()) if not severity_values.empty else self._default_value('collisionSeverity')