import matplotlib.pyplot as plt
import numpy as np
import math
from algorithm.planner.utils.car_utils import Car_C

PI = np.pi


class Arrow:
    def __init__(self, x, y, theta, L, c):
        angle = np.deg2rad(30)
        d = 0.3 * L
        w = 2

        x_start = x
        y_start = y
        x_end = x + L * np.cos(theta)
        y_end = y + L * np.sin(theta)

        theta_hat_L = theta + PI - angle
        theta_hat_R = theta + PI + angle

        x_hat_start = x_end
        x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)
        x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)

        y_hat_start = y_end
        y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)
        y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)

        plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w)
        plt.plot([x_hat_start, x_hat_end_L],
                 [y_hat_start, y_hat_end_L], color=c, linewidth=w)
        plt.plot([x_hat_start, x_hat_end_R],
                 [y_hat_start, y_hat_end_R], color=c, linewidth=w)


class Car:
    def __init__(self, x, y, yaw, w, L):
        theta_B = PI + yaw

        xB = x + L / 4 * np.cos(theta_B)
        yB = y + L / 4 * np.sin(theta_B)

        theta_BL = theta_B + PI / 2
        theta_BR = theta_B - PI / 2

        x_BL = xB + w / 2 * np.cos(theta_BL)        # Bottom-Left vertex
        y_BL = yB + w / 2 * np.sin(theta_BL)
        x_BR = xB + w / 2 * np.cos(theta_BR)        # Bottom-Right vertex
        y_BR = yB + w / 2 * np.sin(theta_BR)

        x_FL = x_BL + L * np.cos(yaw)               # Front-Left vertex
        y_FL = y_BL + L * np.sin(yaw)
        x_FR = x_BR + L * np.cos(yaw)               # Front-Right vertex
        y_FR = y_BR + L * np.sin(yaw)

        plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],
                 [y_BL, y_BR, y_FR, y_FL, y_BL],
                 linewidth=1, color='black')

        Arrow(x, y, yaw, L / 2, 'black')


def draw_car(x, y, yaw, steer, color='black', extended_car=True):

    if extended_car:
        car = np.array([[-Car_C.RB, -Car_C.RB, Car_C.RF, Car_C.RF, -Car_C.RB, Car_C.ACTUAL_RF, Car_C.ACTUAL_RF, -Car_C.ACTUAL_RB, -Car_C.ACTUAL_RB],
                    [Car_C.W / 2, -Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2, Car_C.W / 2, Car_C.W/2, -Car_C.W/2, -Car_C.W/2, Car_C.W/2]])
    else:
        car = np.array([[-Car_C.RB, -Car_C.RB, Car_C.RF, Car_C.RF, -Car_C.RB],
                    [Car_C.W / 2, -Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2, Car_C.W / 2]])

    wheel = np.array([[-Car_C.TR, -Car_C.TR, Car_C.TR, Car_C.TR, -Car_C.TR],
                      [Car_C.TW / 4, -Car_C.TW / 4, -Car_C.TW / 4, Car_C.TW / 4, Car_C.TW / 4]])

    rlWheel = wheel.copy()
    rrWheel = wheel.copy()
    frWheel = wheel.copy()
    flWheel = wheel.copy()

    Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)],
                     [math.sin(yaw), math.cos(yaw)]])

    Rot2 = np.array([[math.cos(steer), math.sin(steer)],
                     [-math.sin(steer), math.cos(steer)]])

    frWheel = np.dot(Rot2, frWheel)
    flWheel = np.dot(Rot2, flWheel)

    frWheel += np.array([[Car_C.WB], [-Car_C.WD / 2]])
    flWheel += np.array([[Car_C.WB], [Car_C.WD / 2]])
    rrWheel[1, :] -= Car_C.WD / 2
    rlWheel[1, :] += Car_C.WD / 2

    frWheel = np.dot(Rot1, frWheel)
    flWheel = np.dot(Rot1, flWheel)

    rrWheel = np.dot(Rot1, rrWheel)
    rlWheel = np.dot(Rot1, rlWheel)
    car = np.dot(Rot1, car)

    frWheel += np.array([[x], [y]])
    flWheel += np.array([[x], [y]])
    rrWheel += np.array([[x], [y]])
    rlWheel += np.array([[x], [y]])
    car += np.array([[x], [y]])

    plt.plot(car[0, :], car[1, :], color)
    plt.plot(frWheel[0, :], frWheel[1, :], color)
    plt.plot(rrWheel[0, :], rrWheel[1, :], color)
    plt.plot(flWheel[0, :], flWheel[1, :], color)
    plt.plot(rlWheel[0, :], rlWheel[1, :], color)
    Arrow(x, y, yaw, Car_C.WB * 0.8, color)


[((2498, 2641), 'numpy.array', 'np.array', (['[[-Car_C.TR, -Car_C.TR, Car_C.TR, Car_C.TR, -Car_C.TR], [Car_C.TW / 4, -\n    Car_C.TW / 4, -Car_C.TW / 4, Car_C.TW / 4, Car_C.TW / 4]]'], {}), '([[-Car_C.TR, -Car_C.TR, Car_C.TR, Car_C.TR, -Car_C.TR], [Car_C.TW /\n    4, -Car_C.TW / 4, -Car_C.TW / 4, Car_C.TW / 4, Car_C.TW / 4]])\n', (2506, 2641), True, 'import numpy as np\n'), ((3010, 3031), 'numpy.dot', 'np.dot', (['Rot2', 'frWheel'], {}), '(Rot2, frWheel)\n', (3016, 3031), True, 'import numpy as np\n'), ((3046, 3067), 'numpy.dot', 'np.dot', (['Rot2', 'flWheel'], {}), '(Rot2, flWheel)\n', (3052, 3067), True, 'import numpy as np\n'), ((3084, 3123), 'numpy.array', 'np.array', (['[[Car_C.WB], [-Car_C.WD / 2]]'], {}), '([[Car_C.WB], [-Car_C.WD / 2]])\n', (3092, 3123), True, 'import numpy as np\n'), ((3139, 3177), 'numpy.array', 'np.array', (['[[Car_C.WB], [Car_C.WD / 2]]'], {}), '([[Car_C.WB], [Car_C.WD / 2]])\n', (3147, 3177), True, 'import numpy as np\n'), ((3261, 3282), 'numpy.dot', 'np.dot', (['Rot1', 'frWheel'], {}), '(Rot1, frWheel)\n', (3267, 3282), True, 'import numpy as np\n'), ((3297, 3318), 'numpy.dot', 'np.dot', (['Rot1', 'flWheel'], {}), '(Rot1, flWheel)\n', (3303, 3318), True, 'import numpy as np\n'), ((3334, 3355), 'numpy.dot', 'np.dot', (['Rot1', 'rrWheel'], {}), '(Rot1, rrWheel)\n', (3340, 3355), True, 'import numpy as np\n'), ((3370, 3391), 'numpy.dot', 'np.dot', (['Rot1', 'rlWheel'], {}), '(Rot1, rlWheel)\n', (3376, 3391), True, 'import numpy as np\n'), ((3402, 3419), 'numpy.dot', 'np.dot', (['Rot1', 'car'], {}), '(Rot1, car)\n', (3408, 3419), True, 'import numpy as np\n'), ((3436, 3456), 'numpy.array', 'np.array', (['[[x], [y]]'], {}), '([[x], [y]])\n', (3444, 3456), True, 'import numpy as np\n'), ((3472, 3492), 'numpy.array', 'np.array', (['[[x], [y]]'], {}), '([[x], [y]])\n', (3480, 3492), True, 'import numpy as np\n'), ((3508, 3528), 'numpy.array', 'np.array', (['[[x], [y]]'], {}), '([[x], [y]])\n', (3516, 3528), True, 'import numpy as np\n'), ((3544, 3564), 'numpy.array', 'np.array', (['[[x], [y]]'], {}), '([[x], [y]])\n', (3552, 3564), True, 'import numpy as np\n'), ((3576, 3596), 'numpy.array', 'np.array', (['[[x], [y]]'], {}), '([[x], [y]])\n', (3584, 3596), True, 'import numpy as np\n'), ((3602, 3643), 'matplotlib.pyplot.plot', 'plt.plot', (['car[(0), :]', 'car[(1), :]', 'color'], {}), '(car[(0), :], car[(1), :], color)\n', (3610, 3643), True, 'import matplotlib.pyplot as plt\n'), ((3644, 3693), 'matplotlib.pyplot.plot', 'plt.plot', (['frWheel[(0), :]', 'frWheel[(1), :]', 'color'], {}), '(frWheel[(0), :], frWheel[(1), :], color)\n', (3652, 3693), True, 'import matplotlib.pyplot as plt\n'), ((3694, 3743), 'matplotlib.pyplot.plot', 'plt.plot', (['rrWheel[(0), :]', 'rrWheel[(1), :]', 'color'], {}), '(rrWheel[(0), :], rrWheel[(1), :], color)\n', (3702, 3743), True, 'import matplotlib.pyplot as plt\n'), ((3744, 3793), 'matplotlib.pyplot.plot', 'plt.plot', (['flWheel[(0), :]', 'flWheel[(1), :]', 'color'], {}), '(flWheel[(0), :], flWheel[(1), :], color)\n', (3752, 3793), True, 'import matplotlib.pyplot as plt\n'), ((3794, 3843), 'matplotlib.pyplot.plot', 'plt.plot', (['rlWheel[(0), :]', 'rlWheel[(1), :]', 'color'], {}), '(rlWheel[(0), :], rlWheel[(1), :], color)\n', (3802, 3843), True, 'import matplotlib.pyplot as plt\n'), ((201, 215), 'numpy.deg2rad', 'np.deg2rad', (['(30)'], {}), '(30)\n', (211, 215), True, 'import numpy as np\n'), ((757, 823), 'matplotlib.pyplot.plot', 'plt.plot', (['[x_start, x_end]', '[y_start, y_end]'], {'color': 'c', 'linewidth': 'w'}), '([x_start, x_end], [y_start, y_end], color=c, linewidth=w)\n', (765, 823), True, 'import matplotlib.pyplot as plt\n'), ((832, 922), 'matplotlib.pyplot.plot', 'plt.plot', (['[x_hat_start, x_hat_end_L]', '[y_hat_start, y_hat_end_L]'], {'color': 'c', 'linewidth': 'w'}), '([x_hat_start, x_hat_end_L], [y_hat_start, y_hat_end_L], color=c,\n    linewidth=w)\n', (840, 922), True, 'import matplotlib.pyplot as plt\n'), ((944, 1034), 'matplotlib.pyplot.plot', 'plt.plot', (['[x_hat_start, x_hat_end_R]', '[y_hat_start, y_hat_end_R]'], {'color': 'c', 'linewidth': 'w'}), '([x_hat_start, x_hat_end_R], [y_hat_start, y_hat_end_R], color=c,\n    linewidth=w)\n', (952, 1034), True, 'import matplotlib.pyplot as plt\n'), ((1754, 1858), 'matplotlib.pyplot.plot', 'plt.plot', (['[x_BL, x_BR, x_FR, x_FL, x_BL]', '[y_BL, y_BR, y_FR, y_FL, y_BL]'], {'linewidth': '(1)', 'color': '"""black"""'}), "([x_BL, x_BR, x_FR, x_FL, x_BL], [y_BL, y_BR, y_FR, y_FL, y_BL],\n    linewidth=1, color='black')\n", (1762, 1858), True, 'import matplotlib.pyplot as plt\n'), ((2035, 2307), 'numpy.array', 'np.array', (['[[-Car_C.RB, -Car_C.RB, Car_C.RF, Car_C.RF, -Car_C.RB, Car_C.ACTUAL_RF,\n    Car_C.ACTUAL_RF, -Car_C.ACTUAL_RB, -Car_C.ACTUAL_RB], [Car_C.W / 2, -\n    Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2, Car_C.W / 2, Car_C.W / 2, -\n    Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2]]'], {}), '([[-Car_C.RB, -Car_C.RB, Car_C.RF, Car_C.RF, -Car_C.RB, Car_C.\n    ACTUAL_RF, Car_C.ACTUAL_RF, -Car_C.ACTUAL_RB, -Car_C.ACTUAL_RB], [Car_C\n    .W / 2, -Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2, Car_C.W / 2, Car_C.W /\n    2, -Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2]])\n', (2043, 2307), True, 'import numpy as np\n'), ((2330, 2468), 'numpy.array', 'np.array', (['[[-Car_C.RB, -Car_C.RB, Car_C.RF, Car_C.RF, -Car_C.RB], [Car_C.W / 2, -\n    Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2, Car_C.W / 2]]'], {}), '([[-Car_C.RB, -Car_C.RB, Car_C.RF, Car_C.RF, -Car_C.RB], [Car_C.W /\n    2, -Car_C.W / 2, -Car_C.W / 2, Car_C.W / 2, Car_C.W / 2]])\n', (2338, 2468), True, 'import numpy as np\n'), ((315, 328), 'numpy.cos', 'np.cos', (['theta'], {}), '(theta)\n', (321, 328), True, 'import numpy as np\n'), ((353, 366), 'numpy.sin', 'np.sin', (['theta'], {}), '(theta)\n', (359, 366), True, 'import numpy as np\n'), ((519, 538), 'numpy.cos', 'np.cos', (['theta_hat_L'], {}), '(theta_hat_L)\n', (525, 538), True, 'import numpy as np\n'), ((579, 598), 'numpy.cos', 'np.cos', (['theta_hat_R'], {}), '(theta_hat_R)\n', (585, 598), True, 'import numpy as np\n'), ((668, 687), 'numpy.sin', 'np.sin', (['theta_hat_L'], {}), '(theta_hat_L)\n', (674, 687), True, 'import numpy as np\n'), ((728, 747), 'numpy.sin', 'np.sin', (['theta_hat_R'], {}), '(theta_hat_R)\n', (734, 747), True, 'import numpy as np\n'), ((1155, 1170), 'numpy.cos', 'np.cos', (['theta_B'], {}), '(theta_B)\n', (1161, 1170), True, 'import numpy as np\n'), ((1196, 1211), 'numpy.sin', 'np.sin', (['theta_B'], {}), '(theta_B)\n', (1202, 1211), True, 'import numpy as np\n'), ((1314, 1330), 'numpy.cos', 'np.cos', (['theta_BL'], {}), '(theta_BL)\n', (1320, 1330), True, 'import numpy as np\n'), ((1387, 1403), 'numpy.sin', 'np.sin', (['theta_BL'], {}), '(theta_BL)\n', (1393, 1403), True, 'import numpy as np\n'), ((1432, 1448), 'numpy.cos', 'np.cos', (['theta_BR'], {}), '(theta_BR)\n', (1438, 1448), True, 'import numpy as np\n'), ((1506, 1522), 'numpy.sin', 'np.sin', (['theta_BR'], {}), '(theta_BR)\n', (1512, 1522), True, 'import numpy as np\n'), ((1550, 1561), 'numpy.cos', 'np.cos', (['yaw'], {}), '(yaw)\n', (1556, 1561), True, 'import numpy as np\n'), ((1622, 1633), 'numpy.sin', 'np.sin', (['yaw'], {}), '(yaw)\n', (1628, 1633), True, 'import numpy as np\n'), ((1660, 1671), 'numpy.cos', 'np.cos', (['yaw'], {}), '(yaw)\n', (1666, 1671), True, 'import numpy as np\n'), ((1733, 1744), 'numpy.sin', 'np.sin', (['yaw'], {}), '(yaw)\n', (1739, 1744), True, 'import numpy as np\n'), ((2792, 2805), 'math.cos', 'math.cos', (['yaw'], {}), '(yaw)\n', (2800, 2805), False, 'import math\n'), ((2846, 2859), 'math.sin', 'math.sin', (['yaw'], {}), '(yaw)\n', (2854, 2859), False, 'import math\n'), ((2861, 2874), 'math.cos', 'math.cos', (['yaw'], {}), '(yaw)\n', (2869, 2874), False, 'import math\n'), ((2901, 2916), 'math.cos', 'math.cos', (['steer'], {}), '(steer)\n', (2909, 2916), False, 'import math\n'), ((2918, 2933), 'math.sin', 'math.sin', (['steer'], {}), '(steer)\n', (2926, 2933), False, 'import math\n'), ((2976, 2991), 'math.cos', 'math.cos', (['steer'], {}), '(steer)\n', (2984, 2991), False, 'import math\n'), ((2808, 2821), 'math.sin', 'math.sin', (['yaw'], {}), '(yaw)\n', (2816, 2821), False, 'import math\n'), ((2959, 2974), 'math.sin', 'math.sin', (['steer'], {}), '(steer)\n', (2967, 2974), False, 'import math\n')]