pujin_outdoor/comfort.py

#!/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:     2023/06/25
@Summary:           Comfort metrics
"""

import os
import sys
import math
import pandas as pd
import numpy as np
import scipy.signal
import matplotlib.pyplot as plt
plt.rcParams['font.family'] = 'sans-serif'
plt.rcParams['font.sans-serif'] = ['SimHei']  # 或者使用你系统中有的其他支持中文的字体
plt.rcParams['axes.unicode_minus'] = False   # 解决保存图像时负号'-'显示为方块的问题

sys.path.append('../common')
sys.path.append('../modules')
sys.path.append('../results')

from data_info import DataInfoList
from score_weight import cal_score_with_priority, cal_weight_from_80
from common import get_interpolation, score_grade, string_concatenate, replace_key_with_value, _cal_max_min_avg
import matplotlib.pyplot as plt


def peak_valley_decorator(method):
    def wrapper(self, *args, **kwargs):
        peak_valley = self._peak_valley_determination(self.df)
        print("peak_valley is", peak_valley)
        pv_list = self.df.loc[peak_valley, ['simTime', 'speedH']].values.tolist()
        print("pv_list is", pv_list)
        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]
                print("self._peak_valley_judgment(p_last, p_curr) is", self._peak_valley_judgment(p_last, p_curr))
                if self._peak_valley_judgment(p_last, p_curr):
                    print("p_curr is", p_curr, p_last)
                    # method(self, p_curr, p_last)
                    method(self, p_curr, p_last, flag, *args, **kwargs)
                    print("method(self, p_curr, p_last, flag, *args, **kwargs) is", method(self, p_curr, p_last, flag, *args, **kwargs))
                else:
                    p_last = p_curr
                    print("when p_last is", p_last)

            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 Comfort(object):
    """
    Class for achieving comfort metrics for autonomous driving.

    Attributes:
        dataframe: Vehicle driving data, stored in dataframe format.
    """

    def __init__(self, data_processed, scoreModel, resultPath):
        self.eval_data = pd.DataFrame()
        self.data_processed = data_processed
        self.scoreModel = scoreModel
        self.resultPath = resultPath

        # self.data = data_processed.obj_data[1]
        self.data = data_processed.ego_df
        self.mileage = data_processed.report_info['mileage']
        self.ego_df = pd.DataFrame()
        # self.discomfort_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])

        self.config = data_processed.config
        comfort_config = data_processed.comfort_config
        self.comfort_config = comfort_config
        print("self.comfort_config is", self.comfort_config)

        # common data
        self.bulitin_metric_list = self.config.builtinMetricList

        # dimension data
        self.weight_custom = comfort_config['weightCustom']
        self.metric_list = comfort_config['metric']
        self.type_list = comfort_config['type']
        self.type_name_dict = comfort_config['typeName']
        self.name_dict = comfort_config['name']
        self.unit_dict = comfort_config['unit']

        # custom metric data
        # self.customMetricParam = comfort_config['customMetricParam']
        # self.custom_metric_list = list(self.customMetricParam.keys())
        # self.custom_data = custom_data
        # self.custom_param_dict = {}

        # score data
        self.weight = comfort_config['weightDimension']

        self.weight_type_dict = comfort_config['typeWeight']
        self.weight_type_list = comfort_config['typeWeightList']

        self.weight_dict = comfort_config['weight']
        self.weight_list = comfort_config['weightList']

        self.priority_dict = comfort_config['priority']
        self.priority_list = comfort_config['priorityList']

        self.kind_dict = comfort_config['kind']
        self.optimal_dict = comfort_config['optimal']
        # self.optimal1_dict = self.optimal_dict[0]
        # self.optimal2_dict = self.optimal_dict[1]
        # self.optimal3_dict = self.optimal_dict[2]
        self.multiple_dict = comfort_config['multiple']
        self.kind_list = comfort_config['kindList']
        print("self.kind_list is", self.kind_list)
        self.optimal_list = comfort_config['optimalList']
        print("self.optimal_list is", self.optimal_list)
        self.multiple_list = comfort_config['multipleList']

        # metric data
        self.metric_dict = comfort_config['typeMetricDict']
        print("self.metric_dict is", self.metric_dict)
        # print("self.metric_dict is", self.metric_dict)
        self.zigzag_metric_list = self.metric_dict['zigzag']

        # self.cadence_metric_list = self.metric_dict['cadence']
        # self.zigzag_metric_list = self.metric_dict['comfortLat']['zigzag']
        # print("self.zigzag_metric_list is", self.zigzag_metric_list)
        self.cadence_metric_list = ["cadence", "slamBrake", "slamAccelerate"]

        self.time_list = data_processed.driver_ctrl_data['time_list']
        self.frame_list = data_processed.driver_ctrl_data['frame_list']
        self.speed_list = []
        self.commandSpeed_list = []
        self.accel_list = []
        self.accelH_list = []

        self.linear_accel_dict = dict()
        self.angular_accel_dict = dict()
        self.speed_instruction_dict = dict()

        self.count_dict = {}
        self.duration_dict = {}
        self.strength_dict = {}

        self.discomfort_count = 0
        self.zigzag_count = 0
        # self.shake_count = 0
        self.cadence_count = 0
        self.slam_brake_count = 0
        self.slam_accel_count = 0
        self.speed_instruction_jump_count = 0

        self.zigzag_time = []
        # self.shake_count = 0
        self.cadence_time = []
        self.slam_brake_time = []
        self.slam_accel_time = []
        self.speed_instruction_jump_time = []

        # self.zigzag_strength = 0
        # self.shake_strength = 0
        # self.cadence_strength = 0
        # self.slam_brake_strength = 0
        # self.slam_accel_strength = 0
        #
        self.discomfort_duration = 0
        # self.zigzag_duration = 0
        # self.shake_duration = 0
        # self.cadence_duration = 0
        # self.slam_brake_duration = 0
        # self.slam_accel_duration = 0

        self.zigzag_time_list = []
        # self.zigzag_frame_list = []
        self.zigzag_stre_list = []
        self.cur_ego_path_list = []
        self.curvature_list = []
        self.discomfort_count_list = []

        self._get_data()
        self._comf_param_cal()

    def _get_data(self):
        """

        """
        comfort_info_list = DataInfoList.COMFORT_INFO
        self.ego_df = self.data[comfort_info_list].copy()
        # self.df = self.ego_df.set_index('simFrame')  # 索引是csv原索引
        self.df = self.ego_df.reset_index(drop=True)  # 索引是csv原索引

    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 _comf_param_cal(self):
        """

        """
        # for i in range(len(self.optimal_list)):
        #     if i % 3 == 2:
        #         continue
        #     else:
        #         self.optimal_list[i] = round(self.optimal_list[i] * self.mileage / 100000, 8)

        # self.optimal_list = [round(self.optimal_list[i] * self.mileage / 100000, 8) for i in range(len(self.optimal_list))]
        self.optimal_dict = {key: value * self.mileage / 100000 for key, value in self.optimal_dict.copy().items()}
        # self.optimal1_dict = {key: value * self.mileage / 100000 for key, value in self.optimal1_dict.copy().items()}
        # self.optimal2_dict = {key: value * self.mileage / 100000 for key, value in self.optimal2_dict.copy().items()}

        # [log]
        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.apply(
            lambda row: self._slam_brake_process(row['lon_acc'], row['ip_dec']), axis=1)
        self.ego_df['slam_accel'] = self.ego_df.apply(
            lambda row: self._slam_accelerate_process(row['lon_acc'], row['ip_acc']), axis=1)

        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)

        self.speed_list = self.ego_df['v'].values.tolist()
        self.commandSpeed_list = self.ego_df['cmd_lon_v'].values.tolist()
        self.accel_list = self.ego_df['accel'].values.tolist()
        self.accelH_list = self.ego_df['accelH'].values.tolist()

        v_jump_threshold = 0.5
        self.ego_df['cmd_lon_v_diff'] = self.ego_df['cmd_lon_v'].diff()
        self.ego_df['cmd_v_jump'] = (abs(self.ego_df['cmd_lon_v_diff']) > v_jump_threshold).astype(int)

        self.linear_accel_dict = _cal_max_min_avg(self.ego_df['accel'].dropna().values.tolist())
        self.angular_accel_dict = _cal_max_min_avg(self.ego_df['accelH'].dropna().values.tolist())
        self.speed_instruction_dict = _cal_max_min_avg(self.ego_df['cmd_lon_v_diff'].dropna().values.tolist())

    def _peak_valley_determination(self, df):
        """
        Determine the peak and valley of the vehicle based on its current angular velocity.

        Parameters:
            df: Dataframe containing the vehicle angular velocity.

        Returns:
            peak_valley: List of indices representing peaks and valleys.
        """

        peaks, _ = scipy.signal.find_peaks(df['speedH'], height=0.01, distance=1, prominence=0.01)
        valleys, _ = scipy.signal.find_peaks(-df['speedH'], height=0.01, distance=1, prominence=0.01)
        peak_valley = sorted(list(peaks) + list(valleys))

        return peak_valley

    def is_not_on_any_interval_endpoint(self, number, intervals):
        # 遍历所有给定的区间
        for interval in intervals:
            interval_start, interval_end = interval

            # 检查数字是否等于区间的起始值或结束值
            if number == interval_start or number == interval_end:
                return False  # 如果在任何一个区间的端点上，则返回False

        # 如果不在任何区间的端点上，则返回True
        return True

    def _peak_valley_judgment(self, p_last, p_curr, tw=6000, avg=0.4):
        """
        Determine if the given peaks and valleys satisfy certain conditions.
        Parameters:
            p_last: Previous peak or valley data point.
            p_curr: Current peak or valley data point.
            tw: Threshold time difference between peaks and valleys.
            avg: Angular velocity gap threshold.

        Returns:
            Boolean indicating whether the conditions are satisfied.
        """
        t_diff = p_curr[0] - p_last[0]
        v_diff = abs(p_curr[1] - p_last[1])
        s = p_curr[1] * p_last[1]

        zigzag_flag = t_diff < tw and v_diff > avg and s < 0
        if zigzag_flag and self.is_not_on_any_interval_endpoint(p_last[0], self.zigzag_time_list):
            self.zigzag_time_list.append([p_last[0], p_curr[0]])
        self.zigzag_time = self.zigzag_time_list
        return zigzag_flag

    @peak_valley_decorator
    def zigzag_count_func(self, p_curr, p_last, flag=True):
        """
        Count the number of zigzag movements.

        Parameters:
            df: Input dataframe data.

        Returns:
            zigzag_count: Number of zigzag movements.
        """
        if flag:
            self.zigzag_count += 1
        else:
            self.zigzag_count += 0

    @peak_valley_decorator
    def cal_zigzag_strength_strength(self, p_curr, p_last, flag=True):
        """
        Calculate various strength statistics.

        Returns:
            Tuple containing maximum strength, minimum strength,
            average strength, and 99th percentile strength.
        """
        if flag:
            v_diff = abs(p_curr[1] - p_last[1])
            t_diff = p_curr[0] - p_last[0]
            self.zigzag_stre_list.append(v_diff / t_diff)  # 平均角加速度
        else:
            self.zigzag_stre_list = []

    def _cadence_process(self, lon_acc_roc, ip_dec_roc):
        if abs(lon_acc_roc) >= abs(ip_dec_roc) or abs(lon_acc_roc) < 1:
            return np.nan
        # elif abs(lon_acc_roc) == 0:
        elif abs(lon_acc_roc) == 0:
            return 0
        elif lon_acc_roc > 0 and lon_acc_roc < -ip_dec_roc:
            return 1
        elif lon_acc_roc < 0 and lon_acc_roc > ip_dec_roc:
            return -1

    def _slam_brake_process(self, lon_acc, ip_dec):
        if lon_acc - ip_dec < 0:
            return 1
        else:
            return 0

    def _slam_accelerate_process(self, lon_acc, ip_acc):
        if lon_acc - ip_acc > 0:
            return 1
        else:
            return 0

    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_roc) == 0:
        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

    def _cadence_detector(self):
        """
        # 加速度突变：先加后减，先减后加，先加然后停，先减然后停
        # 顿挫：2s内多次加速度变化率突变
        # 求出每一个特征点，然后提取，然后将每一个特征点后面的2s做一个窗口，统计频率，避免无效运算

        # 将特征点筛选出来
        # 将特征点时间作为聚类标准，大于1s的pass，小于等于1s的聚类到一个分组
        # 去掉小于3个特征点的分组
        """
        # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'cadence']].copy()
        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()

        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] <= 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)
        print("group_time is", group_time)
        group_frame.append(sub_group_frame)
        group_time = [g for g in group_time if len(g) >= 1]  # 有一次特征点则算作一次顿挫
        print("group_time_new is", group_time)
        # 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]

        # 将顿挫组的起始时间为组重新统计时间
        cadence_time_list = [time for pair in cadence_time for time in time_list if pair[0] <= time <= pair[1]]
        print("cadence_time_list is", group_time)
        # 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
        self.cadence_time = group_time
        return cadence_time_list

    def _slam_brake_detector(self):
        # 统计急刹全为1的分段的个数，记录分段开头的frame_ID
        # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'ip_dec_roc', 'slam_brake']].copy()
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'lon_acc_roc', 'ip_dec', 'slam_brake']].copy()
        # data['slam_diff'] = data['slam_brake'].diff()
        # res_df = data[data['slam_diff'] == 1]

        res_df = data[data['slam_brake'] == 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 sub_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]

        time_list = [element for sublist in group_time for element in sublist]
        self.slam_brake_count = len(group_time)  # / self.mileage  # * 1000000
        self.slam_brake_time = group_time
        return time_list

    def _slam_accel_detector(self):
        # 统计急刹全为1的分段的个数，记录分段开头的frame_ID
        # data = self.ego_df[['simTime', 'simFrame', 'lon_acc_roc', 'ip_acc_roc', 'slam_accel']].copy()
        data = self.ego_df[['simTime', 'simFrame', 'lon_acc', 'ip_acc', 'slam_accel']].copy()
        # data['slam_diff'] = data['slam_accel'].diff()
        # res_df = data.loc[data['slam_diff'] == 1]

        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]

        time_list = [element for sublist in group_time for element in sublist]
        self.slam_accel_count = len(group_time)  # / self.mileage  # * 1000000
        print("slam_accel group_time is", group_time)
        self.slam_accel_time = group_time
        return time_list

    def _speed_instruction_jump_detector(self):

        data = self.ego_df[['simTime', 'simFrame', 'cmd_lon_v', 'cmd_lon_v_diff', 'cmd_v_jump']].copy()
        # data['slam_diff'] = data['slam_accel'].diff()
        # res_df = data.loc[data['slam_diff'] == 1]

        res_df = data.loc[data['cmd_v_jump'] == 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] <= 10:  # 连续帧的算作同一组跳变
                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]

        time_list = [element for sublist in group_time for element in sublist]
        self.speed_instruction_jump_count = len(group_time)  # / self.mileage  # * 1000000
        print("speed_instruction_jump group_time is", group_time)
        self.speed_instruction_jump_time = group_time
        return time_list

    def comf_statistic(self):
        """

        """
        # df = self.ego_df[['simTime', 'cur_diff', 'lon_acc', 'lon_acc_roc', 'accelH']].copy()
        df = self.ego_df[['simTime', 'lon_acc', 'lon_acc_roc', 'accelH']].copy()

        self.zigzag_count_func()
        self.cal_zigzag_strength_strength()
        # if self.zigzag_time_list:
        #     zigzag_df = pd.DataFrame(self.zigzag_time_list, columns=['start_time', 'end_time'])
        #     zigzag_df = get_frame_with_time(zigzag_df, self.ego_df)
        #     zigzag_df['type'] = 'zigzag'
        # self.discomfort_df = pd.concat([self.discomfort_df, zigzag_df], ignore_index=True)
        # discomfort_df = pd.concat([time_df, frame_df], axis=1)
        # self.discomfort_df = pd.concat([self.discomfort_df, discomfort_df], ignore_index=True)

        zigzag_t_list = []
        # 只有[t_start, t_end]数对，要提取为完整time list
        t_list = df['simTime'].values.tolist()
        for t_start, t_end in self.zigzag_time_list:
            index_1 = t_list.index(t_start)
            index_2 = t_list.index(t_end)
            zigzag_t_list.extend(t_list[index_1:index_2 + 1])
        zigzag_t_list = list(set(zigzag_t_list))
        # shake_t_list = self._shake_detector()
        cadence_t_list = self._cadence_detector()
        print("cadence_t_list is", cadence_t_list)
        slam_brake_t_list = self._slam_brake_detector()
        slam_accel_t_list = self._slam_accel_detector()
        speed_instruction_jump_t_list = self._speed_instruction_jump_detector()
        print("speed_instruction_jump_t_list is", speed_instruction_jump_t_list)

        discomfort_time_list = zigzag_t_list + cadence_t_list + slam_brake_t_list + slam_accel_t_list + speed_instruction_jump_t_list
        discomfort_time_list = sorted(discomfort_time_list)  # 排序
        discomfort_time_list = list(set(discomfort_time_list))  # 去重

        time_diff = self.time_list[3] - self.time_list[2]
        # time_diff = 0.4

        self.discomfort_duration = len(discomfort_time_list) * time_diff

        self.count_dict = {
            "zigzag": len(self.zigzag_time),
            # "shake": self.shake_count,
            "cadence": self.cadence_count,
            "slamBrake": self.slam_brake_count,
            "slamAccelerate": self.slam_accel_count,
            "speedInstructionJump": self.speed_instruction_jump_count
        }

        tmp_comf_arr = [len(self.zigzag_time), self.cadence_count, self.slam_brake_count, self.slam_accel_count,
                        self.speed_instruction_jump_count]

        self.discomfort_count = sum(tmp_comf_arr)
        self.discomfort_count_list = [len(self.zigzag_time), self.cadence_count, self.slam_brake_count, self.slam_accel_count,
                        self.speed_instruction_jump_count]
        comf_arr = [tmp_comf_arr]
        return comf_arr

    # def _nan_detect(self, num):
    #     if math.isnan(num):
    #         return 0
    #     return num

    def comf_score_new(self):
        arr_comf = self.comf_statistic()
        print("\n[平顺性表现及得分情况]")
        print("平顺性各指标值：", [[round(num, 2) for num in row] for row in arr_comf])
        arr_comf = np.array(arr_comf)

        # score_model = self.scoreModel(self.kind_list, self.optimal_list, self.multiple_list, arr_comf)
        # score_sub = score_model.cal_score()
        # print("score_sub is", score_sub)
        score_metric = list(map(lambda x: 0 if (x>0) else 100, self.discomfort_count_list))
        # score_metric = list(map(lambda x: 80 if np.isnan(x) else x, score_sub))
        print("score_metric is", score_metric)
        metric_list = [x for x in self.metric_list if x in self.config.builtinMetricList]
        print("metric_list is", metric_list)
        score_metric_dict = {key: value for key, value in zip(metric_list, score_metric)}

        # custom_metric_list = list(self.customMetricParam.keys())
        # for metric in custom_metric_list:
        #     value = self.custom_data[metric]['value']
        #     param_list = self.customMetricParam[metric]
        #     score = self.custom_metric_score(metric, value, param_list)
        #     score_metric_dict[metric] = round(score, 2)

        # score_metric_dict = {key: score_metric_dict[key] for key in self.metric_list}
        # score_metric = list(score_metric_dict.values())

        score_type_dict = {}
        if self.weight_custom:  # 自定义权重
            score_metric_with_weight_dict = {key: score_metric_dict[key] * self.weight_dict[key] for key in
                                             self.weight_dict}
            print("score_metric_with_weight_dict is", score_metric_with_weight_dict)
            print("self.weight_dict is", self.weight_dict)
            for type1 in self.type_list:
                # type_score = sum(
                #     value for key, value in score_metric_with_weight_dict.items() if key in self.metric_dict[type])
                score_list = [float(value) for key, value in score_metric_with_weight_dict.items() if key in self.metric_dict[type1]]
                type_score = sum(score_list)
                print("type_score is", "{:.2f}".format(type_score))
                score_type_dict[type1] = type_score

            score_type_with_weight_dict = {key: score_type_dict[key] * self.weight_type_dict[key] for key in
                                           score_type_dict}

            score_comfort = sum(score_type_with_weight_dict.values())
        else:  # 客观赋权
            self.weight_list = cal_weight_from_80(score_metric)
            self.weight_dict = {key: value for key, value in zip(self.metric_list, self.weight_list)}
            score_comfort = cal_score_with_priority(score_metric, self.weight_list, self.priority_list)

            for type1 in self.type_list:
                type_weight = sum(value for key, value in self.weight_dict.items() if key in self.metric_dict[type1])
                self.weight_dict = {key: round(value / type_weight, 4) for key, value in self.weight_dict.items() if
                                    key in self.metric_dict[type1]}
                type_score_metric = [value for key, value in score_metric_dict.items() if key in self.metric_dict[type1]]
                type_weight_list = [value for key, value in self.weight_dict.items() if key in self.metric_dict[type1]]
                type_priority_list = [value for key, value in self.priority_dict.items() if
                                      key in self.metric_dict[type1]]

                type_score = cal_score_with_priority(type_score_metric, type_weight_list, type_priority_list)
                score_type_dict[type1] = np.around(type_score, decimals = 2)

        score_comfort = round(score_comfort, 2)

        print("平顺性各指标基准值：", self.optimal_list)
        print(f"平顺性得分为：{score_comfort:.2f}分。")
        print(f"平顺性各类型得分为：{score_type_dict}。")
        print(f"平顺性各指标得分为：{score_metric_dict}。")
        return score_comfort, score_type_dict, score_metric_dict

    def zip_time_pairs(self, zip_list, upper_limit=9999):
        zip_time_pairs = zip(self.time_list, zip_list)
        zip_vs_time = [[x, upper_limit if y > upper_limit else y] for x, y in zip_time_pairs if not math.isnan(y)]
        return zip_vs_time

    def report_statistic(self):
        """

        Returns:

        """
        # report_dict = {
        #     "name": "平顺性",
        #     "weight": f"{self.weight * 100:.2f}%",
        #     "weightDistribution": weight_distribution,
        #     "score": score_comfort,
        #     "level": grade_comfort,

        #     'discomfortCount': self.discomfort_count,
        #     "description1": comf_description1,
        #     "description2": comf_description2,
        #     "description3": comf_description3,
        #     "description4": comf_description4,
        #
        #     "comfortLat": lat_dict,
        #     "comfortLon": lon_dict,
        #
        #     "speData": ego_speed_vs_time,
        #     "speMarkLine": discomfort_slices,
        #
        #     "accData": lon_acc_vs_time,
        #     "accMarkLine": discomfort_acce_slices,
        #
        #     "anvData": yawrate_vs_time,
        #     "anvMarkLine": discomfort_zigzag_slices,
        #
        #     "anaData": yawrate_roc_vs_time,
        #     "anaMarkLine": discomfort_zigzag_slices,
        #
        #     "curData": [cur_ego_path_vs_time, curvature_vs_time],
        #     "curMarkLine": discomfort_shake_slices,
        # }
        # brakePedal_list = self.data_processed.driver_ctrl_data['brakePedal_list']
        # throttlePedal_list = self.data_processed.driver_ctrl_data['throttlePedal_list']
        # steeringWheel_list = self.data_processed.driver_ctrl_data['steeringWheel_list']
        #
        # # common parameter calculate
        # brake_vs_time = self.zip_time_pairs(brakePedal_list, 100)
        # throttle_vs_time = self.zip_time_pairs(throttlePedal_list, 100)
        # steering_vs_time = self.zip_time_pairs(steeringWheel_list)

        report_dict = {
            "name": "平顺性",
            "weight": f"{self.weight * 100:.2f}%",

            # 'discomfortCount': self.discomfort_count,
        }

        score_comfort, score_type_dict, score_metric_dict = self.comf_score_new()
        score_comfort = int(score_comfort) if int(score_comfort) == score_comfort else round(score_comfort, 2)
        grade_comfort = score_grade(score_comfort)
        report_dict["score"] = score_comfort
        report_dict["level"] = grade_comfort

        description = f"· 在平顺性方面，得分{score_comfort:.2f}分，表现{grade_comfort}。"
        is_good = True
        is_semicolon = False

        if any(score_metric_dict[metric] < 80 for metric in self.cadence_metric_list):
            is_good = False
            is_semicolon = True
            description += "线加速度变化剧烈，"
            tmp = [metric for metric in self.cadence_metric_list if score_metric_dict[metric] < 80]
            str_tmp = "、".join(tmp)
            description += f"有{str_tmp}情况，需重点优化"

        if any(score_metric_dict[metric] < 80 for metric in self.zigzag_metric_list):
            is_good = False
            if is_semicolon:
                description += "；"
                is_semicolon = False
            description += "角加速度变化剧烈，"
            tmp = [metric for metric in self.zigzag_metric_list if score_metric_dict[metric] < 80]
            str_tmp = "、".join(tmp)
            description += f"有{str_tmp}情况，需重点优化。"
        else:
            if is_semicolon:
                description += f"。"

        if is_good:
            description += f"线加速度和角加速度变化平顺，表现{grade_comfort}。"

        report_dict["description"] = replace_key_with_value(description, self.name_dict)

        # indexes

        description1 = f"次数：{len(self.zigzag_time)}次"
        if len(self.zigzag_time) > 0:
            print("self.zigzag_time is", self.zigzag_time)
            tmp_zigzag = [str("{:.2f}".format(t_zigzag[0]))+'s' for t_zigzag in self.zigzag_time]
            str_tmp_zigzag = "、".join(tmp_zigzag)
            description1 += f"\n在{str_tmp_zigzag}处发生画龙行为"
        else:
            description1 += f"\n没有发生画龙行为"

        description2 = f"次数：{self.cadence_count}次"
        if len(self.cadence_time) > 0:
            print("self.zigzag_time is", self.cadence_time)
            tmp_cadence = [str("{:.2f}".format(t_cadence[0]))+'s' for t_cadence in self.cadence_time]
            str_tmp_cadence = "、".join(tmp_cadence)
            description2 += f"\n在{str_tmp_cadence}处发生顿挫行为"
        else:
            description2 += f"\n没有发生顿挫行为"

        description3 = f"次数: {self.slam_brake_count + self.slam_accel_count}次"
        if len(self.slam_brake_time) > 0:
            # print("self.zigzag_time is", self.cadence_time)
            tmp_slam_brake = [str("{:.2f}".format(t_slam_brake[0]))+'s' for t_slam_brake in self.slam_brake_time]
            str_tmp_slam_brake = "、".join(tmp_slam_brake)
            description3 += f"\n在{str_tmp_slam_brake}处发生急刹行为"
        else:
            description3 += f"\n没有发生急刹行为"
        if len(self.slam_accel_time) > 0:
            slam_accel_brake = [str("{:.2f}".format(t_slam_accel[0]))+'s' for t_slam_accel in self.slam_accel_time]
            str_tmp_slam_accel = "、".join(slam_accel_brake)
            description3 += f"\n在{str_tmp_slam_accel}处发生急加速行为"
        else:

            description3 += f"\n没有发生急加速行为"

        description4 = f"次数: {self.speed_instruction_jump_count}次"
        if len(self.speed_instruction_jump_time) > 0:
            print("self.zigzag_time is", self.cadence_time)
            tmp_speed_instruction_jump = [str("{:.2f}".format(t_speed_instruction_jump[0]))+'s' for t_speed_instruction_jump in self.speed_instruction_jump_time]
            str_tmp_speed_instruction_jump = "、".join(tmp_speed_instruction_jump)
            description4 += f"\n在{str_tmp_speed_instruction_jump}处发生指令跳变"
        else:
            description4 += f"\n没有发生指令跳变"

        zigzag_index = {
            "weight": self.weight_type_dict['zigzag'],
            "score": score_type_dict['zigzag'],
            "description": description1
        }

        cadence_index = {
            "weight": self.weight_type_dict['cadence'],
            "score": score_type_dict['cadence'],
            "description": description2
        }

        sharpchangeofspeed_index = {
            "weight": self.weight_type_dict['sharpchangeofspeed'],
            "score": score_type_dict['sharpchangeofspeed'],
            "description": description3
        }

        speedInstruction_index = {
            "weight": self.weight_type_dict['comfortSpeed'],
            "score": score_type_dict['comfortSpeed'],
            "description": description4
        }

        indexes_dict = {
            "zigzag": zigzag_index,
            "cadence": cadence_index,
            "sharpchangeofspeed": sharpchangeofspeed_index,
            "comfortSpeed": speedInstruction_index
        }

        report_dict["indexes"] = indexes_dict

        # LinearAccelerate.png
        plt.figure(figsize=(12, 3))
        plt.plot(self.time_list, self.accel_list)

        plt.xlabel('时间(s)')
        plt.ylabel('线加速度(m/s^2)')
        # plt.legend()

        # 调整布局，消除空白边界
        plt.tight_layout()

        plt.savefig(os.path.join(self.resultPath, "LinearAccelerate.png"))
        plt.close()

        # AngularAccelerate.png
        plt.figure(figsize=(12, 3))
        plt.plot(self.time_list, self.accelH_list)

        plt.xlabel('时间(s)')
        plt.ylabel('角加速度(rad/s^2)')
        # plt.legend()

        # 调整布局，消除空白边界
        plt.tight_layout()

        plt.savefig(os.path.join(self.resultPath, "AngularAccelerate.png"))
        plt.close()

        # Speed.png
        plt.figure(figsize=(12, 3))
        plt.plot(self.time_list, self.speed_list)

        plt.xlabel('时间(s)')
        plt.ylabel('速度(m/s)')
        # plt.legend()

        # 调整布局，消除空白边界
        plt.tight_layout()

        plt.savefig(os.path.join(self.resultPath, "Speed.png"))
        plt.close()

        # commandSpeed.png draw
        plt.figure(figsize=(12, 3))
        plt.plot(self.time_list, self.commandSpeed_list)

        plt.xlabel('时间(s)')
        plt.ylabel('控制指令速度(m/s)')
        # plt.legend()

        # 调整布局，消除空白边界
        plt.tight_layout()

        plt.savefig(os.path.join(self.resultPath, "CommandSpeed.png"))
        plt.close()

        print(report_dict)
        return report_dict

    # def get_eval_data(self):
    #     df = self.eval_data[
    #         ['simTime', 'simFrame', 'playerId', 'ip_acc', 'ip_dec', 'slam_brake', 'slam_accel', 'cadence']].copy()
    #     return df