'''
-------------------------------------------------------------------------------------------------
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
[((764, 783), 'numpy.cos', 'np.cos', (['delta_angle'], {}), '(delta_angle)\n', (770, 783), True, 'import numpy as np\n'), ((1091, 1125), 'numpy.arctan', 'np.arctan', (['(veh_cgf[0] / veh_cgs[0])'], {}), '(veh_cgf[0] / veh_cgs[0])\n', (1100, 1125), True, 'import numpy as np\n'), ((1147, 1183), 'numpy.abs', 'np.abs', (['(veh_cgs[1] - veh_striking[3])'], {}), '(veh_cgs[1] - veh_striking[3])\n', (1153, 1183), True, 'import numpy as np\n'), ((1620, 1654), 'numpy.arctan', 'np.arctan', (['(veh_cgf[0] / veh_cgs[0])'], {}), '(veh_cgf[0] / veh_cgs[0])\n', (1629, 1654), True, 'import numpy as np\n'), ((1676, 1712), 'numpy.abs', 'np.abs', (['(veh_cgf[1] - veh_striking[3])'], {}), '(veh_cgf[1] - veh_striking[3])\n', (1682, 1712), True, 'import numpy as np\n'), ((1276, 1318), 'numpy.sqrt', 'np.sqrt', (['(veh_cgf[0] ** 2 + veh_cgs[0] ** 2)'], {}), '(veh_cgf[0] ** 2 + veh_cgs[0] ** 2)\n', (1283, 1318), True, 'import numpy as np\n'), ((1321, 1351), 'numpy.cos', 'np.cos', (['(veh_ca + delta_angle_2)'], {}), '(veh_ca + delta_angle_2)\n', (1327, 1351), True, 'import numpy as np\n'), ((1859, 1880), 'numpy.sin', 'np.sin', (['delta_angle_2'], {}), '(delta_angle_2)\n', (1865, 1880), True, 'import numpy as np\n'), ((1972, 2006), 'numpy.arctan', 'np.arctan', (['(veh_cgf[1] / veh_cgs[1])'], {}), '(veh_cgf[1] / veh_cgs[1])\n', (1981, 2006), True, 'import numpy as np\n'), ((2028, 2064), 'numpy.abs', 'np.abs', (['(veh_cgs[0] - veh_striking[3])'], {}), '(veh_cgs[0] - veh_striking[3])\n', (2034, 2064), True, 'import numpy as np\n'), ((1220, 1239), 'numpy.cos', 'np.cos', (['delta_angle'], {}), '(delta_angle)\n', (1226, 1239), True, 'import numpy as np\n'), ((1741, 1783), 'numpy.sqrt', 'np.sqrt', (['(veh_cgf[0] ** 2 + veh_cgs[0] ** 2)'], {}), '(veh_cgf[0] ** 2 + veh_cgs[0] ** 2)\n', (1748, 1783), True, 'import numpy as np\n'), ((1786, 1828), 'numpy.cos', 'np.cos', (['(delta_angle_2 - veh_ca + np.pi / 2)'], {}), '(delta_angle_2 - veh_ca + np.pi / 2)\n', (1792, 1828), True, 'import numpy as np\n'), ((2501, 2535), 'numpy.arctan', 'np.arctan', (['(veh_cgf[1] / veh_cgs[1])'], {}), '(veh_cgf[1] / veh_cgs[1])\n', (2510, 2535), True, 'import numpy as np\n'), ((2557, 2593), 'numpy.abs', 'np.abs', (['(veh_cgf[0] - veh_striking[3])'], {}), '(veh_cgf[0] - veh_striking[3])\n', (2563, 2593), True, 'import numpy as np\n'), ((2157, 2199), 'numpy.sqrt', 'np.sqrt', (['(veh_cgf[1] ** 2 + veh_cgs[1] ** 2)'], {}), '(veh_cgf[1] ** 2 + veh_cgs[1] ** 2)\n', (2164, 2199), True, 'import numpy as np\n'), ((2202, 2232), 'numpy.cos', 'np.cos', (['(veh_ca + delta_angle_2)'], {}), '(veh_ca + delta_angle_2)\n', (2208, 2232), True, 'import numpy as np\n'), ((2740, 2761), 'numpy.sin', 'np.sin', (['delta_angle_2'], {}), '(delta_angle_2)\n', (2746, 2761), True, 'import numpy as np\n'), ((2101, 2120), 'numpy.cos', 'np.cos', (['delta_angle'], {}), '(delta_angle)\n', (2107, 2120), True, 'import numpy as np\n'), ((2622, 2664), 'numpy.sqrt', 'np.sqrt', (['(veh_cgf[1] ** 2 + veh_cgs[1] ** 2)'], {}), '(veh_cgf[1] ** 2 + veh_cgs[1] ** 2)\n', (2629, 2664), True, 'import numpy as np\n'), ((2667, 2709), 'numpy.cos', 'np.cos', (['(delta_angle_2 - veh_ca + np.pi / 2)'], {}), '(delta_angle_2 - veh_ca + np.pi / 2)\n', (2673, 2709), True, 'import numpy as np\n')]