#!/usr/bin/env python
# -*- coding: utf-8 -*-
##################################################################
#
# Copyright (c) 2023 CICV, Inc. All Rights Reserved
#
##################################################################
"""
@Authors:           zhanghaiwen(zhanghaiwen@china-icv.cn), yangzihao(yangzihao@china-icv.cn)
@Data:              2023/06/25
@Last Modified:     2025/04/25
@Summary:           Comfort metrics
"""

import sys
import math
import scipy.signal
import pandas as pd
import numpy as np
from pathlib import Path 
from typing import Dict, List, Any, Optional, Callable, Union, Tuple

from modules.lib.score import Score
from modules.lib.common import get_interpolation, get_frame_with_time
from modules.lib import data_process

from modules.lib.log_manager import LogManager

COMFORT_INFO = [
    "simTime",
    "simFrame",
    "speedX",
    "speedY",
    "accelX",
    "accelY",
    "curvHor",
    "lightMask",
    "v",
    "lat_acc",
    "lon_acc",
    "time_diff",
    "lon_acc_diff",
    "lon_acc_roc",
    "speedH",
    "accelH",
    "posH",  # 确保包含航向角
]
# ----------------------
# 独立指标计算函数
# ----------------------
# 添加 Motion Sickness Dose Value (MSDV) 指标
#用于量化乘员因持续振动而产生的晕动风险。以下是需要添加的代码：

## 1. 在独立指标计算函数部分添加新函数
def calculate_ava_vav(data_processed) -> dict:
    """计算多维度综合加权加速度"""
    comfort = ComfortCalculator(data_processed)
    ava_vav_value = comfort.calculate_ava_vav()
    return {"ava_vav": float(ava_vav_value)}

def calculate_msdv(data_processed) -> dict:
    """计算晕动剂量值(MSDV)指标"""
    comfort = ComfortCalculator(data_processed)
    msdv_value = comfort.calculate_msdv()
    return {"msdv": float(msdv_value)}
    
def calculate_weaving(data_processed) -> dict:
    """计算蛇行指标"""
    comfort = ComfortCalculator(data_processed)
    zigzag_count = comfort.calculate_zigzag_count()
    return {"weaving": float(zigzag_count)}

def calculate_shake(data_processed) -> dict:
    """计算晃动指标"""
    comfort = ComfortCalculator(data_processed)
    shake_count = comfort.calculate_shake_count()
    return {"shake": float(shake_count)}

def calculate_cadence(data_processed) -> dict:
    """计算顿挫指标"""
    comfort = ComfortCalculator(data_processed)
    cadence_count = comfort.calculate_cadence_count()
    return {"cadence": float(cadence_count)}

def calculate_slambrake(data_processed) -> dict:
    """计算急刹车指标"""
    comfort = ComfortCalculator(data_processed)
    slam_brake_count = comfort.calculate_slam_brake_count()
    return {"slamBrake": float(slam_brake_count)}

def calculate_slamaccelerate(data_processed) -> dict:
    """计算急加速指标"""
    comfort = ComfortCalculator(data_processed)
    slam_accel_count = comfort.calculate_slam_accel_count()
    return {"slamAccelerate": float(slam_accel_count)}

# 装饰器保持不变
def peak_valley_decorator(method):
    def wrapper(self, *args, **kwargs):
        peak_valley = self._peak_valley_determination(self.df)
        pv_list = self.df.loc[peak_valley, ['simTime', 'speedH']].values.tolist()
        if len(pv_list) != 0:
            flag = True
            p_last = pv_list[0]

            for i in range(1, len(pv_list)):
                p_curr = pv_list[i]

                if self._peak_valley_judgment(p_last, p_curr):
                    # method(self, p_curr, p_last)
                    method(self, p_curr, p_last, flag, *args, **kwargs)
                else:
                    p_last = p_curr

            return method
        else:
            flag = False
            p_curr = [0, 0]
            p_last = [0, 0]
            method(self, p_curr, p_last, flag, *args, **kwargs)
            return method

    return wrapper


class ComfortRegistry:
    """舒适性指标注册器"""
    
    def __init__(self, data_processed):
        self.logger = LogManager().get_logger()  # 获取全局日志实例
        self.data = data_processed
        self.comfort_config = data_processed.comfort_config["comfort"]
        self.metrics = self._extract_metrics(self.comfort_config)
        self._registry = self._build_registry()
    
    def _extract_metrics(self, config_node: dict) -> list:
        """DFS遍历提取指标"""
        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()}"
            try:
                registry[metric_name] = globals()[func_name]
            except KeyError:
                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)
                # 新增：将每个指标的结果写入日志
                self.logger.info(f'舒适性指标[{name}]计算结果: {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 ComfortCalculator:
    """舒适性指标计算类 - 提供核心计算功能"""
    
    def __init__(self, data_processed):
        self.data_processed = data_processed
        self.logger = LogManager().get_logger()
        
        self.data = data_processed.ego_data
        self.ego_df = pd.DataFrame()
        self.discomfort_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])
        
        # 统计指标
        self.calculated_value = {
            'weaving': 0, 
            'shake': 0, 
            'cadence': 0,
            'slamBrake': 0, 
            'slamAccelerate': 0,
            'ava_vav': 0,  # 添加新指标的默认值
            'msdv': 0      # 添加MSDV指标的默认值
        }
        
        self.time_list = self.data['simTime'].values.tolist()
        self.frame_list = self.data['simFrame'].values.tolist()
        
        self.zigzag_count = 0
        self.shake_count = 0
        self.cadence_count = 0
        self.slam_brake_count = 0
        self.slam_accel_count = 0
        
        self.zigzag_time_list = []
        self.zigzag_stre_list = []
        
        self._initialize_data()
    
    def _initialize_data(self):
        """初始化数据"""
        self.ego_df = self.data[COMFORT_INFO].copy()
        self.df = self.ego_df.reset_index(drop=True)
        self._prepare_comfort_parameters()
    
    def _prepare_comfort_parameters(self):
        """准备舒适性计算所需参数"""
        # 计算加减速阈值
        self.ego_df['ip_acc'] = self.ego_df['v'].apply(get_interpolation, point1=[18, 4], point2=[72, 2])
        self.ego_df['ip_dec'] = self.ego_df['v'].apply(get_interpolation, point1=[18, -5], point2=[72, -3.5])
        self.ego_df['slam_brake'] = (self.ego_df['lon_acc'] - self.ego_df['ip_dec']).apply(
            lambda x: 1 if x < 0 else 0)
        self.ego_df['slam_accel'] = (self.ego_df['lon_acc'] - self.ego_df['ip_acc']).apply(
            lambda x: 1 if x > 0 else 0)
        self.ego_df['cadence'] = self.ego_df.apply(
            lambda row: self._cadence_process_new(row['lon_acc'], row['ip_acc'], row['ip_dec']), axis=1)

    
    def _cal_cur_ego_path(self, row):
        """计算车辆轨迹曲率"""
        try:
            divide = (row['speedX'] ** 2 + row['speedY'] ** 2) ** (3 / 2)
            if not divide:
                res = None
            else:
                res = (row['speedX'] * row['accelY'] - row['speedY'] * row['accelX']) / divide
        except:
            res = None
        return res
    
    def _peak_valley_determination(self, df):
        """确定角速度的峰谷"""
        peaks, _ = scipy.signal.find_peaks(
            df['speedH'], height=2.3, distance=3, 
            prominence=2.3, width=1)
        valleys, _ = scipy.signal.find_peaks(
            -df['speedH'], height=2.3, distance=3, 
            prominence=2.3, width=1)
        return sorted(list(peaks) + list(valleys))
    
    def _peak_valley_judgment(self, p_last, p_curr, tw=100, avg=4.6):
        """判断峰谷是否满足蛇行条件"""
        t_diff = p_curr[0] - p_last[0]
        v_diff = abs(p_curr[1] - p_last[1])
        s = p_curr[1] * p_last[1]

        if t_diff < tw and v_diff > avg and s < 0:
            if [p_last[0], p_curr[0]] not in self.zigzag_time_list:
                self.zigzag_time_list.append([p_last[0], p_curr[0]])
            return True
        return False
    
    def _cadence_process_new(self, lon_acc, ip_acc, ip_dec):
        """处理顿挫数据"""
        if abs(lon_acc) < 1 or lon_acc > ip_acc or lon_acc < ip_dec:
            return np.nan
        elif abs(lon_acc) == 0:
            return 0
        elif lon_acc > 0 and lon_acc < ip_acc:
            return 1
        elif lon_acc < 0 and lon_acc > ip_dec:
            return -1
        else:
            return 0
    
    @peak_valley_decorator
    def _zigzag_count_func(self, p_curr, p_last, flag=True):
        """计算蛇行次数"""
        if flag:
            self.zigzag_count += 1
        else:
            self.zigzag_count += 0
    
    @peak_valley_decorator
    def _cal_zigzag_strength(self, p_curr, p_last, flag=True):
        """计算蛇行强度"""
        if flag:
            v_diff = abs(p_curr[1] - p_last[1])
            t_diff = p_curr[0] - p_last[0]
            if t_diff > 0:
                self.zigzag_stre_list.append(v_diff / t_diff)  # 平均角加速度
        else:
            self.zigzag_stre_list = []
    
    def _get_zigzag_times(self):
        """获取所有蛇行时间点"""
        all_times = []
        for time_range in self.zigzag_time_list:
            start, end = time_range
            # 获取这个时间范围内的所有时间点
            times_in_range = self.ego_df[(self.ego_df['simTime'] >= start) & 
                                         (self.ego_df['simTime'] <= end)]['simTime'].tolist()
            all_times.extend(times_in_range)
        return all_times
    
    def calculate_zigzag_count(self):
        """计算蛇行指标"""
        self._zigzag_count_func()
        return self.zigzag_count
    
    def calculate_shake_count(self):
        """计算晃动指标"""
        self._shake_detector()
        return self.shake_count
    
    def calculate_cadence_count(self):
        """计算顿挫指标"""
        self._cadence_detector()
        return self.cadence_count
    
    def calculate_slam_brake_count(self):
        """计算急刹车指标"""
        self._slam_brake_detector()
        return self.slam_brake_count
    
    def calculate_slam_accel_count(self):
        """计算急加速指标"""
        self._slam_accel_detector()
        return self.slam_accel_count
    
    def _shake_detector(self, T_diff=0.5):
        """检测晃动事件 - 改进版本（不使用车辆轨迹曲率）"""
        # lat_acc已经是车辆坐标系下的横向加速度，由data_process.py计算
        time_list = []
        frame_list = []

        # 复制数据以避免修改原始数据
        df = self.ego_df.copy()
        
        # 1. 计算横向加速度变化率
        df['lat_acc_rate'] = df['lat_acc'].diff() / df['simTime'].diff()
        
        # 2. 计算横摆角速度变化率
        df['speedH_rate'] = df['speedH'].diff() / df['simTime'].diff()
        
        # 3. 计算横摆角速度的短期变化特性
        window_size = 5  # 5帧窗口
        df['speedH_std'] = df['speedH'].rolling(window=window_size, min_periods=2).std()
        
        # 4. 基于车速的动态阈值
        v0 = 20 * 5/18        # ≈5.56 m/s
        # 递减系数
        k  = 0.008 * 3.6      # =0.0288 per m/s
        df['lat_acc_threshold'] = df['v'].apply(
            lambda speed: max(
                1.0,                                   # 下限 1.0 m/s²
                min(
                    1.8,                               # 上限 1.8 m/s²
                    1.8 - k * (speed - v0)             # 线性递减
                )
            )
        )
        
        df['speedH_threshold'] = df['v'].apply(
            lambda speed: max(1.5, min(3.0, 2.0 * (1 + (speed - 20) / 60)))
        )
        # 将计算好的阈值和中间变量保存到self.ego_df中，供其他函数使用
        self.ego_df['lat_acc_threshold'] = df['lat_acc_threshold']
        self.ego_df['speedH_threshold'] = df['speedH_threshold']
        self.ego_df['lat_acc_rate'] = df['lat_acc_rate']
        self.ego_df['speedH_rate'] = df['speedH_rate'] 
        self.ego_df['speedH_std'] = df['speedH_std']
        
        # 5. 综合判断晃动条件
        # 条件A: 横向加速度超过阈值
        condition_A = df['lat_acc'].abs() > df['lat_acc_threshold']
        
        # 条件B: 横向加速度变化率超过阈值
        lat_acc_rate_threshold = 0.5  # 横向加速度变化率阈值 (m/s³)
        condition_B = df['lat_acc_rate'].abs() > lat_acc_rate_threshold
        
        # 条件C: 横摆角速度有明显变化但不呈现周期性
        condition_C = (df['speedH_std'] > df['speedH_threshold']) & (~df['simTime'].isin(self._get_zigzag_times()))
        
        # 综合条件: 满足条件A，且满足条件B或条件C
        shake_condition = condition_A & (condition_B | condition_C)
        
        # 筛选满足条件的数据
        shake_df = df[shake_condition].copy()
        
        # 按照连续帧号分组，确保只有连续帧超过阈值的才被认为是晃动
        if not shake_df.empty:
            shake_df['frame_diff'] = shake_df['simFrame'].diff().fillna(0)
            shake_df['group'] = (shake_df['frame_diff'] > T_diff).cumsum()
            
            # 分组统计
            shake_groups = shake_df.groupby('group')
            
            for _, group in shake_groups:
                if len(group) >= 2:  # 至少2帧才算一次晃动
                    time_list.extend(group['simTime'].values)
                    frame_list.extend(group['simFrame'].values)
                    self.shake_count += 1
        
        # 分组处理
        TIME_RANGE = 1
        t_list = time_list
        f_list = frame_list
        group_time = []
        group_frame = []
        sub_group_time = []
        sub_group_frame = []
        
        if len(f_list) > 0:
            for i in range(len(f_list)):
                if not sub_group_time or t_list[i] - t_list[i - 1] <= TIME_RANGE:
                    sub_group_time.append(t_list[i])
                    sub_group_frame.append(f_list[i])
                else:
                    group_time.append(sub_group_time)
                    group_frame.append(sub_group_frame)
                    sub_group_time = [t_list[i]]
                    sub_group_frame = [f_list[i]]

            group_time.append(sub_group_time)
            group_frame.append(sub_group_frame)

        # 输出图表值
        shake_time = [[g[0], g[-1]] for g in group_time]
        shake_frame = [[g[0], g[-1]] for g in group_frame]
        self.shake_count = len(shake_time)

        if shake_time:
            time_df = pd.DataFrame(shake_time, columns=['start_time', 'end_time'])
            frame_df = pd.DataFrame(shake_frame, columns=['start_frame', 'end_frame'])
            discomfort_df = pd.concat([time_df, frame_df], axis=1)
            discomfort_df['type'] = 'shake'
            self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)

        return time_list
    
    def _cadence_detector(self):
        """顿挫检测器"""
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'cadence']].copy()
        time_list = data['simTime'].values.tolist()

        data = data[data['cadence'] != np.nan]
        data['cadence_diff'] = data['cadence'].diff()
        data.dropna(subset='cadence_diff', inplace=True)
        data = data[data['cadence_diff'] != 0]

        t_list = data['simTime'].values.tolist()
        f_list = data['simFrame'].values.tolist()

        TIME_RANGE = 1
        group_time = []
        group_frame = []
        sub_group_time = []
        sub_group_frame = []
        for i in range(len(f_list)):
            if not sub_group_time or t_list[i] - t_list[i - 1] <= TIME_RANGE:  # 特征点相邻一秒内的，算作同一组顿挫
                sub_group_time.append(t_list[i])
                sub_group_frame.append(f_list[i])
            else:
                group_time.append(sub_group_time)
                group_frame.append(sub_group_frame)
                sub_group_time = [t_list[i]]
                sub_group_frame = [f_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) >= 1]  # 有一次特征点则算作一次顿挫
        group_frame = [g for g in group_frame if len(g) >= 1]

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

        if cadence_time:
            time_df = pd.DataFrame(cadence_time, columns=['start_time', 'end_time'])
            frame_df = pd.DataFrame(cadence_frame, columns=['start_frame', 'end_frame'])
            discomfort_df = pd.concat([time_df, frame_df], axis=1)
            discomfort_df['type'] = 'cadence'
            self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)

        # 将顿挫组的起始时间为组重新统计时间
        cadence_time_list = [time for pair in cadence_time for time in time_list if pair[0] <= time <= pair[1]]

        stre_list = []
        freq_list = []
        for g in group_time:
            # calculate strength
            g_df = data[data['simTime'].isin(g)]
            strength = g_df['lon_acc'].abs().mean()
            stre_list.append(strength)

            # calculate frequency
            cnt = len(g)
            t_start = g_df['simTime'].iloc[0]
            t_end = g_df['simTime'].iloc[-1]
            t_delta = t_end - t_start
            frequency = cnt / t_delta
            freq_list.append(frequency)

        self.cadence_count = len(freq_list)
        cadence_stre = sum(stre_list) / len(stre_list) if stre_list else 0

        return cadence_time_list
    
    def _slam_brake_detector(self):
        """急刹车检测器"""
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'ip_dec', 'slam_brake']].copy()
        res_df = data[data['slam_brake'] == 1]
        t_list = res_df['simTime'].values
        f_list = res_df['simFrame'].values.tolist()

        TIME_RANGE = 1
        group_time = []
        group_frame = []
        sub_group_time = []
        sub_group_frame = []
        for i in range(len(f_list)):
            if not sub_group_time or f_list[i] - f_list[i - 1] <= TIME_RANGE:  # 连续帧的算作同一组急刹
                sub_group_time.append(t_list[i])
                sub_group_frame.append(f_list[i])
            else:
                group_time.append(sub_group_time)
                group_frame.append(sub_group_frame)
                sub_group_time = [t_list[i]]
                sub_group_frame = [f_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]

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

        if slam_brake_time:
            time_df = pd.DataFrame(slam_brake_time, columns=['start_time', 'end_time'])
            frame_df = pd.DataFrame(slam_brake_frame, columns=['start_frame', 'end_frame'])
            discomfort_df = pd.concat([time_df, frame_df], axis=1)
            discomfort_df['type'] = 'slam_brake'
            self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)

        time_list = [element for sublist in group_time for element in sublist]
        self.slam_brake_count = len(group_time)
        return time_list
    
    def _slam_accel_detector(self):
        """急加速检测器"""
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'ip_acc', 'slam_accel']].copy()
        res_df = data.loc[data['slam_accel'] == 1]
        t_list = res_df['simTime'].values
        f_list = res_df['simFrame'].values.tolist()

        group_time = []
        group_frame = []
        sub_group_time = []
        sub_group_frame = []
        for i in range(len(f_list)):
            if not group_time or f_list[i] - f_list[i - 1] <= 1:  # 连续帧的算作同一组急加速
                sub_group_time.append(t_list[i])
                sub_group_frame.append(f_list[i])
            else:
                group_time.append(sub_group_time)
                group_frame.append(sub_group_frame)
                sub_group_time = [t_list[i]]
                sub_group_frame = [f_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]

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

        if slam_accel_time:
            time_df = pd.DataFrame(slam_accel_time, columns=['start_time', 'end_time'])
            frame_df = pd.DataFrame(slam_accel_frame, columns=['start_frame', 'end_frame'])
            discomfort_df = pd.concat([time_df, frame_df], axis=1)
            discomfort_df['type'] = 'slam_accel'
            self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)

        time_list = [element for sublist in group_time for element in sublist]
        self.slam_accel_count = len(group_time)
        return time_list


class ComfortManager:
    """舒适性指标计算主类"""
    
    def __init__(self, data_processed):
        self.data = data_processed
        self.logger = LogManager().get_logger()
        self.registry = ComfortRegistry(self.data)

    def report_statistic(self):
        """生成舒适性评分报告"""
        comfort_result = self.registry.batch_execute()
        
        return comfort_result


if __name__ == '__main__':
    case_name = 'ICA'
    mode_label = 'PGVIL'
    
    data = data_process.DataPreprocessing(case_name, mode_label)
    comfort_instance = ComfortManager(data)
    
    try:  
        comfort_result = comfort_instance.report_statistic() 
        result = {'comfort': comfort_result}
        print(result) 
    except Exception as e:  
        print(f"An error occurred in Comfort.report_statistic: {e}")

# 将之前定义在类外部的方法移动到ComfortCalculator类内部
def _apply_frequency_weighting(self, acceleration_data, weighting_type='Wk', fs=100):
    """应用ISO 2631-1:1997标准的频率加权滤波
    
    参数:
        acceleration_data: 加速度时间序列数据
        weighting_type: 加权类型，可选值包括:
            - 'Wk': 垂直方向(Z轴)加权
            - 'Wd': 水平方向(X和Y轴)加权
            - 'Wf': 运动病相关加权
        fs: 采样频率(Hz)
        
    返回:
        加权后的加速度数据
    """
    # 检查数据有效性
    if acceleration_data.empty or acceleration_data.isna().all():
        return acceleration_data
        
    # 根据ISO 2631-1:1997标准设计滤波器
    # 这些参数来自标准文档，用于构建数字滤波器
    if weighting_type == 'Wk':  # 垂直方向(Z轴)
        # Wk滤波器参数
        f1 = 0.4
        f2 = 100.0
        f3 = 12.5
        f4 = 12.5
        Q1 = 0.63
        Q2 = 0.5
        Q3 = 0.63
        Q4 = 0.63
        K = 0.4
    elif weighting_type == 'Wd':  # 水平方向(X和Y轴)
        # Wd滤波器参数
        f1 = 0.4
        f2 = 100.0
        f3 = 2.0
        f4 = 2.0
        Q1 = 0.63
        Q2 = 0.5
        Q3 = 0.63
        Q4 = 0.63
        K = 0.4
    elif weighting_type == 'Wf':  # 运动病相关
        # Wf滤波器参数
        f1 = 0.08
        f2 = 0.63
        f3 = 0.25
        f4 = 0.8
        Q1 = 0.63
        Q2 = 0.86
        Q3 = 0.8
        Q4 = 0.8
        K = 1.0
    else:
        self.logger.warning(f"未知的加权类型: {weighting_type}，使用原始数据")
        return acceleration_data
    
    # 将频率转换为角频率
    w1 = 2 * np.pi * f1
    w2 = 2 * np.pi * f2
    w3 = 2 * np.pi * f3
    w4 = 2 * np.pi * f4
    
    # 设计高通滤波器(s域)
    b1 = [K * w1**2, 0]
    a1 = [1, w1/Q1, w1**2]
    
    # 设计低通滤波器(s域)
    b2 = [K, 0, 0]
    a2 = [1, w2/Q2, w2**2]
    
    # 设计加速度-速度转换滤波器(s域)
    b3 = [K, 0]
    a3 = [1, w3/Q3, w3**2]
    
    # 设计上升滤波器(s域)
    b4 = [K, 0, 0]
    a4 = [1, w4/Q4, w4**2]
    
    # 使用双线性变换将s域滤波器转换为z域
    b1_z, a1_z = scipy.signal.bilinear(b1, a1, fs)
    b2_z, a2_z = scipy.signal.bilinear(b2, a2, fs)
    b3_z, a3_z = scipy.signal.bilinear(b3, a3, fs)
    b4_z, a4_z = scipy.signal.bilinear(b4, a4, fs)
    
    # 应用滤波器链
    data_np = acceleration_data.to_numpy()
    filtered_data = scipy.signal.lfilter(b1_z, a1_z, data_np)
    filtered_data = scipy.signal.lfilter(b2_z, a2_z, filtered_data)
    filtered_data = scipy.signal.lfilter(b3_z, a3_z, filtered_data)
    filtered_data = scipy.signal.lfilter(b4_z, a4_z, filtered_data)
    
    return pd.Series(filtered_data, index=acceleration_data.index)

def calculate_ava_vav(self):
    """计算多维度综合加权加速度
    
    基于ISO 2631-1:1997标准，综合考虑车辆在三个平移方向和三个旋转方向的加速度或角速度
    
    Returns:
        float: 多维度综合加权加速度值
    """
    # 定义各方向的权重系数
    k_x = 1.0  # X方向加速度权重
    k_y = 1.0  # Y方向加速度权重
    k_z = 1.0  # Z方向加速度权重
    k_roll = 0.63  # 横滚角速度权重
    k_pitch = 0.8  # 俯仰角速度权重
    k_yaw = 0.5  # 偏航角速度权重
    
    # 获取数据
    df = self.ego_df.copy()
    
    # 确保有必要的列
    if 'accelX' not in df.columns or 'accelY' not in df.columns:
        self.logger.warning("缺少计算多维度综合加权加速度所需的数据列")
        return self.calculated_value['ava_vav']
    
    # 将东北天坐标系下的加速度转换为车身坐标系下的加速度
    # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
    if 'posH' not in df.columns:
        self.logger.warning("缺少航向角数据，无法进行坐标转换")
        return self.calculated_value['ava_vav']
        
    df['posH_rad'] = np.radians(df['posH'])
    
    # 转换加速度到车身坐标系
    # 注意：posH是航向角，北向为0度，顺时针为正
    # 车身X轴 = 东向*sin(posH) + 北向*cos(posH)
    # 车身Y轴 = 东向*cos(posH) - 北向*sin(posH)
    df['a_x_body'] = df['accelX'] * np.sin(df['posH_rad']) + df['accelY'] * np.cos(df['posH_rad'])
    df['a_y_body'] = df['accelX'] * np.cos(df['posH_rad']) - df['accelY'] * np.sin(df['posH_rad'])
    
    # Z方向加速度，如果没有则假设为0
    df['a_z_body'] = df['accelZ'] if 'accelZ' in df.columns else pd.Series(np.zeros(len(df)))
    
    # 角速度数据，如果没有则使用角速度变化率代替
    # 注意：speedH是航向角速度，需要转换为车身坐标系下的偏航角速度
    omega_roll = df['rollRate'] if 'rollRate' in df.columns else pd.Series(np.zeros(len(df)))
    omega_pitch = df['pitchRate'] if 'pitchRate' in df.columns else pd.Series(np.zeros(len(df)))
    omega_yaw = df['speedH']  # 使用航向角速度作为偏航角速度
    
    # 应用ISO 2631-1:1997标准的频率加权滤波
    # 估计采样频率 - 假设数据是均匀采样的
    if len(df) > 1:
        time_diff = df['simTime'].diff().median()
        fs = 1.0 / time_diff if time_diff > 0 else 100  # 默认100Hz
    else:
        fs = 100  # 默认采样频率
    
    # 对各方向加速度应用适当的频率加权
    a_x_weighted = self._apply_frequency_weighting(df['a_x_body'], 'Wd', fs)
    a_y_weighted = self._apply_frequency_weighting(df['a_y_body'], 'Wd', fs)
    a_z_weighted = self._apply_frequency_weighting(df['a_z_body'], 'Wk', fs)
    
    # 对角速度也应用适当的频率加权
    # 注意：ISO标准没有直接指定角速度的加权，这里使用简化处理
    omega_roll_weighted = omega_roll  # 可以根据需要应用适当的滤波
    omega_pitch_weighted = omega_pitch
    omega_yaw_weighted = omega_yaw
    
    # 计算加权均方根值 (r.m.s.)
    # 对每个方向的加速度/角速度平方后求平均，再开平方根
    a_x_rms = np.sqrt(np.mean(a_x_weighted**2))
    a_y_rms = np.sqrt(np.mean(a_y_weighted**2))
    a_z_rms = np.sqrt(np.mean(a_z_weighted**2))
    omega_roll_rms = np.sqrt(np.mean(omega_roll_weighted**2))
    omega_pitch_rms = np.sqrt(np.mean(omega_pitch_weighted**2))
    omega_yaw_rms = np.sqrt(np.mean(omega_yaw_weighted**2))
    
    # 计算综合加权加速度
    ava_vav = np.sqrt(
        k_x * a_x_rms**2 + 
        k_y * a_y_rms**2 + 
        k_z * a_z_rms**2 + 
        k_roll * omega_roll_rms**2 + 
        k_pitch * omega_pitch_rms**2 + 
        k_yaw * omega_yaw_rms**2
    )
    
    # 记录计算结果
    self.calculated_value['ava_vav'] = ava_vav
    self.logger.info(f"多维度综合加权加速度(ava_vav)计算结果: {ava_vav}")
    
    return ava_vav


def calculate_msdv(self):
        """计算晕动剂量值(Motion Sickness Dose Value, MSDV)
        
        MSDV用于量化乘员因持续振动而产生的晕动风险，其物理意义是
        "频率加权后的加速度有效值的平方对时间的累积"，
        能够反映乘员在一定时间内受到振动刺激的总量。
        
        计算公式: MSDV = ∫[0,T] |a_ω(t)|² dt
        
        Returns:
            float: 晕动剂量值
        """
        # 获取数据
        df = self.ego_df.copy()
        
        # 确保有必要的列
        if 'accelX' not in df.columns or 'accelY' not in df.columns:
            self.logger.warning("缺少计算晕动剂量值所需的数据列")
            return self.calculated_value['msdv']
        
        # 将东北天坐标系下的加速度转换为车身坐标系下的加速度
        if 'posH' not in df.columns:
            self.logger.warning("缺少航向角数据，无法进行坐标转换")
            return self.calculated_value['msdv']
            
        # 车身坐标系：X轴指向车头，Y轴指向车辆左侧，Z轴指向车顶
        df['posH_rad'] = np.radians(df['posH'])
        
        # 转换加速度到车身坐标系
        # 注意：posH是航向角，北向为0度，顺时针为正
        # 车身X轴 = 东向*sin(posH) + 北向*cos(posH)
        # 车身Y轴 = 东向*cos(posH) - 北向*sin(posH)
        df['a_x_body'] = df['accelX'] * np.sin(df['posH_rad']) + df['accelY'] * np.cos(df['posH_rad'])
        df['a_y_body'] = df['accelX'] * np.cos(df['posH_rad']) - df['accelY'] * np.sin(df['posH_rad'])
        
        # Z方向加速度，如果没有则假设为0
        df['a_z_body'] = df['accelZ'] if 'accelZ' in df.columns else pd.Series(np.zeros(len(df)))
        
        # 计算时间差
        df['time_diff'] = df['simTime'].diff().fillna(0)
        
        # 应用ISO 2631-1:1997标准的频率加权滤波
        # 估计采样频率 - 假设数据是均匀采样的
        if len(df) > 1:
            time_diff = df['simTime'].diff().median()
            fs = 1.0 / time_diff if time_diff > 0 else 100  # 默认100Hz
        else:
            fs = 100  # 默认采样频率
        
        # 对各方向加速度应用适当的频率加权
        # 对于晕动评估，使用Wf加权滤波器
        a_x_weighted = self._apply_frequency_weighting(df['a_x_body'], 'Wf', fs)
        a_y_weighted = self._apply_frequency_weighting(df['a_y_body'], 'Wf', fs)
        a_z_weighted = self._apply_frequency_weighting(df['a_z_body'], 'Wf', fs)
        
        # 计算MSDV - 对加速度平方进行时间积分
        # 对于X方向（前后方向）- 主要影响晕动感
        msdv_x = np.sqrt(np.sum(a_x_weighted**2 * df['time_diff']))
        
        # 对于Y方向（左右方向）
        msdv_y = np.sqrt(np.sum(a_y_weighted**2 * df['time_diff']))
        
        # 对于Z方向（上下方向）- 也对晕动有显著影响
        msdv_z = np.sqrt(np.sum(a_z_weighted**2 * df['time_diff']))
        
        # 综合MSDV - 可以使用向量和或加权和
        # 根据ISO 2631标准，垂直方向(Z)的权重通常更高
        msdv = np.sqrt(msdv_x**2 + msdv_y**2 + (1.4 * msdv_z)**2)
        
        # 记录计算结果
        self.calculated_value['msdv'] = msdv
        self.logger.info(f"晕动剂量值(MSDV)计算结果: {msdv}")
        
        return msdv