#!/usr/bin/env python
# -*- coding: utf-8 -*-
##################################################################
#
# Copyright (c) 2023 CICV, Inc. All Rights Reserved
#
##################################################################
"""
@Authors:           zhangyu
@Data:              2024/02/21
@Last Modified:     2024/02/21
@Summary:           The template of custom indicator.
"""

"""
    设计思路：
    最大横向偏移量
    zy_center_distance_expectation
"""
import math
import pandas as pd
import numpy as np
from common import zip_time_pairs, continuous_group
from log import logger

"""import functions"""

# custom metric codes
class CustomMetric(object):
    def __init__(self, all_data, case_name):
        self.data = all_data
        self.optimal_dict = self.data.config
        self.case_name = case_name
        self.markline_df = pd.DataFrame(columns=['start_time', 'end_time', 'start_frame', 'end_frame', 'type'])

        self.df = pd.DataFrame()
        self.ego_df = pd.DataFrame()
        self.df_follow = pd.DataFrame()
        self.roadMark_df = pd.DataFrame()
        self.roadPos_df = pd.DataFrame()


        self.time_list_follow = list()
        self.frame_list_follow = list()
        self.dist_list = list()
        self.dist_deviation_list = list()
        self.dist_deviation_list_full_time = list()

        self.result = {
            "name": "固定方向盘转角车辆跨道时间",
            "value": [],
            # "weight": [],
            "tableData": {
                "avg": "",  # 平均值，或指标值
                "max": "",
                "min": ""
            },
            "reportData": {
                "name": "固定方向盘转角车辆跨道时间（s）",
                # "legend": [], # 如果有多个data，则需要增加data对应的说明，如：["横向加速度", "纵向加速度"]
                "data": [],
                "markLine": [],
                "range": [],
            },
            "statusFlag": {}
        }
        self.run()
        print(f"指标09: 固定方向盘转角车辆跨道时间: {self.result['value']}")

    def data_extract(self):
        self.df = self.data.object_df
        self.ego_df = self.data.object_df[self.data.object_df.playerId == 1]
        self.df_follow = self.df[self.df['LKA_status'] == "Active"].copy()  # 数字3对应LKA的Active
        # self.df_follow = self.df[self.df['ACC_status'] == "Active"].copy()  # 数字3对应ICA的Active
        self.roadMark_df = self.data.road_mark_df
        self.roadPos_df = self.data.road_pos_df

        if self.df_follow.empty:
            self.result['statusFlag']['function_LKA'] = False
        else:
            self.result['statusFlag']['function_LKA'] = True

    def dist(self, x1, y1, x2, y2):
        dis = math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
        return dis

    def solve_equation(self, heading_deviation_abs, GF, BF, omiga):
        a = 0.0  # 初始猜测
        epsilon = 0.001  # 误差容限
        max_iterations = 6283  # 最大迭代次数

        list_demo = []

        for i in range(max_iterations):
            GB = BF * math.cos(heading_deviation_abs) * math.tan(a + heading_deviation_abs) - BF * math.sin(
                heading_deviation_abs)
            fx = math.cos(a) * 2 * GF * BF - GF ** 2 - BF ** 2 - GB ** 2
            if abs(fx) < epsilon:
                list_demo.append(a)
            # 调整猜测范围
            a += 0.001

        if len(list_demo) != 0:
            return list_demo[0] / omiga

        return 10000  # 未找到解

    def fixed_steering_wheel_angle_TLC(self, x):
        heading_deviation_abs = x['heading_deviation_abs']
        GF = x['GF']
        BF = x['BF']
        omiga = x['speedH']
        return self.solve_equation(heading_deviation_abs, GF, BF, omiga)

    def func_laneOffset_abs(self, x):
        return abs(x.laneOffset)

    def data_analyze(self):
        player_df = self.df
        mask = (player_df.playerId == 0) | (player_df.playerId == 1)
        ego_df = player_df[mask].reset_index(drop=True)
        # width_ego = ego_df['dimY']  # 自车宽度e

        road_mark_df = self.roadMark_df
        # 左车道线曲率，右车道线曲率，求二者平均值，计算车道线曲率，再与自车朝向相减
        road_mark_left_df = road_mark_df[road_mark_df.id == 0].reset_index(drop=True)
        road_mark_right_df = road_mark_df[road_mark_df.id == 2].reset_index(drop=True)
        road_mark_left_df['curvHor_left'] = road_mark_left_df['curvHor']
        road_mark_left_df['curvHor_right'] = road_mark_right_df['curvHor']
        road_mark_left_df['curvHor_middle'] = road_mark_left_df[['curvHor_left', 'curvHor_right']].apply( \
            lambda x: (x['curvHor_left'] + x['curvHor_right']) / 2, axis=1)
        ego_df['curvHor_middle'] = road_mark_left_df['curvHor_middle']
        ego_df['heading_deviation_abs'] = ego_df[['curvHor_middle', 'posH']].apply( \
            lambda x: abs(x['posH'] - x['curvHor_middle']), axis=1)  # 偏航角θ

        # laneInfo_df = self.laneInfo_df
        # laneInfo_df = laneInfo_df[laneInfo_df.id == -1].reset_index(drop=True)  # laneInfo_df['width'] 车道宽度

        ego_df['velocity_resultant'] = ego_df[['speedX', 'speedY']].apply( \
            lambda x: math.sqrt(x['speedX'] ** 2 - x['speedY'] ** 2) / 3.6, axis=1)  # 汽车行驶速度v

        roadPos_df = self.roadPos_df
        roadPos_ego_df = roadPos_df[roadPos_df.playerId == 1].reset_index(drop=True)
        roadPos_ego_df['laneOffset_abs'] = roadPos_ego_df.apply(lambda x: self.func_laneOffset_abs(x), axis=1)  # 横向偏移量y0

        merged_df = pd.merge(roadPos_ego_df, ego_df, on='simFrame', how='inner')
        merged_df["laneWidth"] = 3.5
        merged_df['GF'] = merged_df[['velocity_resultant', 'speedH']].apply(lambda x: x['velocity_resultant'] / x['speedH'], axis=1)
        merged_df['GF'] = 10000
        merged_df['AD'] = merged_df[['laneWidth', 'laneOffset_abs', 'dimY', 'heading_deviation_abs']].apply( \
            lambda x: x['laneWidth'] - x['laneOffset_abs'] - x['dimY'] / 2 * math.cos(x['heading_deviation_abs']), axis=1)
        merged_df['AB'] = merged_df[['AD', 'heading_deviation_abs']].apply( \
            lambda x: x['AD'] / math.cos(x['heading_deviation_abs']), axis=1)
        merged_df['BF'] = merged_df[['GF', 'AB']].apply(lambda x: x['GF'] - x['AB'], axis=1)

        merged_df['TLC_steer_wheel'] = merged_df.apply(lambda x: self.fixed_steering_wheel_angle_TLC(x), axis=1)
        row_with_min_value = min(merged_df['TLC_steer_wheel'])
        merged_df['simTime'] = merged_df['simTime_x']
        # self.lateral_control11_fixed_steering_wheel_angle_TLC = row_with_min_value
        self.result['value'] = [round(row_with_min_value, 3)]
        self.time_list_follow = merged_df['simTime'].values.tolist()
        self.frame_list_follow = merged_df['simFrame'].values.tolist()
        self.dist_deviation_list = merged_df['TLC_steer_wheel'].values.tolist()


    def markline_statistic(self):
        unfunc_df = pd.DataFrame({'simTime': self.time_list_follow, 'simFrame': self.frame_list_follow,
                                  'dist_deviation': self.dist_deviation_list})
        unfunc_df = unfunc_df[unfunc_df['simFrame'] > 1]
        v_df = unfunc_df
        v_df = v_df[['simTime', 'simFrame', 'dist_deviation']]
        v_follow_df = continuous_group(v_df)
        v_follow_df['type'] = "ICA"
        self.markline_df = pd.concat([self.markline_df, v_follow_df], ignore_index=True)

    def report_data_statistic(self):
        time_list = self.ego_df['simTime'].values.tolist()
        graph_list = [x for x in self.dist_deviation_list if not np.isnan(x)]
        self.result['tableData']['avg'] = f'{np.mean(graph_list):.2f}' if graph_list else 0
        self.result['tableData']['max'] = f'{max(graph_list):.2f}' if graph_list else 0
        self.result['tableData']['min'] = f'{min(graph_list):.2f}' if graph_list else 0

        zip_vs_time = zip_time_pairs(time_list, self.dist_deviation_list)
        self.result['reportData']['data'] = zip_vs_time

        self.markline_statistic()
        markline_slices = self.markline_df.to_dict('records')
        self.result['reportData']['markLine'] = markline_slices
        self.result['reportData']['range'] = f"[-1.875, 1.875]"

    def run(self):
        # logger.info(f"Custom metric run:[{self.result['name']}].")
        logger.info(f"[case:{self.case_name}] Custom metric:[ica_distance_deviation:{self.result['name']}] evaluate.")

        try:
            self.data_extract()
        except Exception as e:
            logger.error(f"[case:{self.case_name}] Custom metric:{self.result['name']} data extract ERROR!", e)

        try:
            self.data_analyze()
        except Exception as e:
            logger.error(f"[case:{self.case_name}] Custom metric:{self.result['name']} data analyze ERROR!", e)

        try:
            self.report_data_statistic()
        except Exception as e:
            logger.error(f"[case:{self.case_name}] Custom metric:{self.result['name']} report data statistic ERROR!", e)

# if __name__ == "__main__":
#     pass
