'''
-------------------------------------------------------------------------------------------------
This code accompanies the paper titled "Human injury-based safety decision of automated vehicles"
Author: Qingfan Wang, Qing Zhou, Miao Lin, Bingbing Nie
Corresponding author: Bingbing Nie (nbb@tsinghua.edu.cn)
-------------------------------------------------------------------------------------------------
'''


import torch
import numpy as np
from torch import nn
from torch.nn.utils import weight_norm


__author__ = "Qingfan Wang"


def Collision_cond(veh_striking_list, V1_v, V2_v, delta_angle, veh_param):
    ''' Estimate the collision condition. '''

    (veh_l, veh_w, veh_cgf, veh_cgs, veh_k, veh_m) = veh_param

    delta_angle_2 = np.arccos(np.abs(np.cos(delta_angle)))
    if -1e-6 < delta_angle_2 < 1e-6:
        delta_angle_2 = 1e-6

    delta_v1_list = []
    delta_v2_list = []

    # Estimate the collision condition (delat-v) according to the principal impact direction.
    for veh_striking in veh_striking_list:
        if veh_striking[0] == 1:
            veh_ca = np.arctan(veh_cgf[0] / veh_cgs[0])
            veh_a2 = np.abs(veh_cgs[1] - veh_striking[3])
            veh_RDS = np.abs(V1_v * np.cos(delta_angle) - V2_v)
            veh_a1 = np.abs(np.sqrt(veh_cgf[0] ** 2 + veh_cgs[0] ** 2) * np.cos(veh_ca + delta_angle_2))
            if (veh_striking[1]+1) in [16, 1, 2, 3, 17, 20, 21] and (veh_striking[2]+1) in [16, 1, 2, 3, 17, 20, 21]:
                veh_e = 2 / veh_RDS
            else:
                veh_e = 0.5 / veh_RDS

        elif veh_striking[0] == 2:
            veh_ca = np.arctan(veh_cgf[0] / veh_cgs[0])
            veh_a2 = np.abs(veh_cgf[1] - veh_striking[3])
            veh_a1 = np.abs(np.sqrt(veh_cgf[0] ** 2 + veh_cgs[0] ** 2) * np.cos(delta_angle_2 - veh_ca + np.pi / 2))
            veh_RDS = V1_v * np.sin(delta_angle_2)
            veh_e = 1.5 / veh_RDS

        elif veh_striking[0] == 3:
            veh_ca = np.arctan(veh_cgf[1] / veh_cgs[1])
            veh_a1 = np.abs(veh_cgs[0] - veh_striking[3])
            veh_RDS = np.abs(V2_v * np.cos(delta_angle) - V1_v)
            veh_a2 = np.abs(np.sqrt(veh_cgf[1] ** 2 + veh_cgs[1] ** 2) * np.cos(veh_ca + delta_angle_2))
            if (veh_striking[1]+1) in [16, 1, 2, 3, 17, 20, 21] and (veh_striking[2]+1) in [16, 1, 2, 3, 17, 20, 21]:
                veh_e = 2 / veh_RDS
            else:
                veh_e = 0.5 / veh_RDS

        elif veh_striking[0] == 4:
            veh_ca = np.arctan(veh_cgf[1] / veh_cgs[1])
            veh_a1 = np.abs(veh_cgf[0] - veh_striking[3])
            veh_a2 = np.abs(np.sqrt(veh_cgf[1] ** 2 + veh_cgs[1] ** 2) * np.cos(delta_angle_2 - veh_ca + np.pi / 2))
            veh_RDS = V2_v * np.sin(delta_angle_2)
            veh_e = 1.5 / veh_RDS


        # Obtain delta-v based on the plane 2-DOF rigid-body collision model with momentum conservation.
        veh_y1 = veh_k[0] ** 2 / (veh_a1 ** 2 + veh_k[0] ** 2)
        veh_y2 = veh_k[1] ** 2 / (veh_a2 ** 2 + veh_k[1] ** 2)
        delta_v1 = (1 + veh_e) * veh_m[1] * veh_y1 * veh_y2 * veh_RDS / (veh_m[0] * veh_y1 + veh_m[1] * veh_y2)
        delta_v2 = (1 + veh_e) * veh_m[0] * veh_y1 * veh_y2 * veh_RDS / (veh_m[0] * veh_y1 + veh_m[1] * veh_y2)

        delta_v1_list.append(delta_v1)
        delta_v2_list.append(delta_v2)

    delta_v1_ = max(delta_v1_list)
    delta_v2_ = max(delta_v2_list)
    index = delta_v1_list.index(max(delta_v1_list))

    return delta_v1_, delta_v2_, index
[((24, 37, 24, 56), 'numpy.cos', 'np.cos', ({(24, 44, 24, 55): 'delta_angle'}, {}), '(delta_angle)', True, 'import numpy as np\n'), ((34, 21, 34, 55), 'numpy.arctan', 'np.arctan', ({(34, 31, 34, 54): 'veh_cgf[0] / veh_cgs[0]'}, {}), '(veh_cgf[0] / veh_cgs[0])', True, 'import numpy as np\n'), ((35, 21, 35, 57), 'numpy.abs', 'np.abs', ({(35, 28, 35, 56): 'veh_cgs[1] - veh_striking[3]'}, {}), '(veh_cgs[1] - veh_striking[3])', True, 'import numpy as np\n'), ((44, 21, 44, 55), 'numpy.arctan', 'np.arctan', ({(44, 31, 44, 54): 'veh_cgf[0] / veh_cgs[0]'}, {}), '(veh_cgf[0] / veh_cgs[0])', True, 'import numpy as np\n'), ((45, 21, 45, 57), 'numpy.abs', 'np.abs', ({(45, 28, 45, 56): 'veh_cgf[1] - veh_striking[3]'}, {}), '(veh_cgf[1] - veh_striking[3])', True, 'import numpy as np\n'), ((37, 28, 37, 70), 'numpy.sqrt', 'np.sqrt', ({(37, 36, 37, 69): 'veh_cgf[0] ** 2 + veh_cgs[0] ** 2'}, {}), '(veh_cgf[0] ** 2 + veh_cgs[0] ** 2)', True, 'import numpy as np\n'), ((37, 73, 37, 103), 'numpy.cos', 'np.cos', ({(37, 80, 37, 102): 'veh_ca + delta_angle_2'}, {}), '(veh_ca + delta_angle_2)', True, 'import numpy as np\n'), ((47, 29, 47, 50), 'numpy.sin', 'np.sin', ({(47, 36, 47, 49): 'delta_angle_2'}, {}), '(delta_angle_2)', True, 'import numpy as np\n'), ((51, 21, 51, 55), 'numpy.arctan', 'np.arctan', ({(51, 31, 51, 54): 'veh_cgf[1] / veh_cgs[1]'}, {}), '(veh_cgf[1] / veh_cgs[1])', True, 'import numpy as np\n'), ((52, 21, 52, 57), 'numpy.abs', 'np.abs', ({(52, 28, 52, 56): 'veh_cgs[0] - veh_striking[3]'}, {}), '(veh_cgs[0] - veh_striking[3])', True, 'import numpy as np\n'), ((36, 36, 36, 55), 'numpy.cos', 'np.cos', ({(36, 43, 36, 54): 'delta_angle'}, {}), '(delta_angle)', True, 'import numpy as np\n'), ((46, 28, 46, 70), 'numpy.sqrt', 'np.sqrt', ({(46, 36, 46, 69): 'veh_cgf[0] ** 2 + veh_cgs[0] ** 2'}, {}), '(veh_cgf[0] ** 2 + veh_cgs[0] ** 2)', True, 'import numpy as np\n'), ((46, 73, 46, 115), 'numpy.cos', 'np.cos', ({(46, 80, 46, 114): 'delta_angle_2 - veh_ca + np.pi / 2'}, {}), '(delta_angle_2 - veh_ca + np.pi / 2)', True, 'import numpy as np\n'), ((61, 21, 61, 55), 'numpy.arctan', 'np.arctan', ({(61, 31, 61, 54): 'veh_cgf[1] / veh_cgs[1]'}, {}), '(veh_cgf[1] / veh_cgs[1])', True, 'import numpy as np\n'), ((62, 21, 62, 57), 'numpy.abs', 'np.abs', ({(62, 28, 62, 56): 'veh_cgf[0] - veh_striking[3]'}, {}), '(veh_cgf[0] - veh_striking[3])', True, 'import numpy as np\n'), ((54, 28, 54, 70), 'numpy.sqrt', 'np.sqrt', ({(54, 36, 54, 69): 'veh_cgf[1] ** 2 + veh_cgs[1] ** 2'}, {}), '(veh_cgf[1] ** 2 + veh_cgs[1] ** 2)', True, 'import numpy as np\n'), ((54, 73, 54, 103), 'numpy.cos', 'np.cos', ({(54, 80, 54, 102): 'veh_ca + delta_angle_2'}, {}), '(veh_ca + delta_angle_2)', True, 'import numpy as np\n'), ((64, 29, 64, 50), 'numpy.sin', 'np.sin', ({(64, 36, 64, 49): 'delta_angle_2'}, {}), '(delta_angle_2)', True, 'import numpy as np\n'), ((53, 36, 53, 55), 'numpy.cos', 'np.cos', ({(53, 43, 53, 54): 'delta_angle'}, {}), '(delta_angle)', True, 'import numpy as np\n'), ((63, 28, 63, 70), 'numpy.sqrt', 'np.sqrt', ({(63, 36, 63, 69): 'veh_cgf[1] ** 2 + veh_cgs[1] ** 2'}, {}), '(veh_cgf[1] ** 2 + veh_cgs[1] ** 2)', True, 'import numpy as np\n'), ((63, 73, 63, 115), 'numpy.cos', 'np.cos', ({(63, 80, 63, 114): 'delta_angle_2 - veh_ca + np.pi / 2'}, {}), '(delta_angle_2 - veh_ca + np.pi / 2)', True, 'import numpy as np\n')]