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)


[((74, 12, 75, 96), 'numpy.array', 'np.array', ({(74, 21, 75, 95): '[[-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]])', True, 'import numpy as np\n'), ((88, 14, 88, 35), 'numpy.dot', 'np.dot', ({(88, 21, 88, 25): 'Rot2', (88, 27, 88, 34): 'frWheel'}, {}), '(Rot2, frWheel)', True, 'import numpy as np\n'), ((89, 14, 89, 35), 'numpy.dot', 'np.dot', ({(89, 21, 89, 25): 'Rot2', (89, 27, 89, 34): 'flWheel'}, {}), '(Rot2, flWheel)', True, 'import numpy as np\n'), ((91, 15, 91, 54), 'numpy.array', 'np.array', ({(91, 24, 91, 53): '[[Car_C.WB], [-Car_C.WD / 2]]'}, {}), '([[Car_C.WB], [-Car_C.WD / 2]])', True, 'import numpy as np\n'), ((92, 15, 92, 53), 'numpy.array', 'np.array', ({(92, 24, 92, 52): '[[Car_C.WB], [Car_C.WD / 2]]'}, {}), '([[Car_C.WB], [Car_C.WD / 2]])', True, 'import numpy as np\n'), ((96, 14, 96, 35), 'numpy.dot', 'np.dot', ({(96, 21, 96, 25): 'Rot1', (96, 27, 96, 34): 'frWheel'}, {}), '(Rot1, frWheel)', True, 'import numpy as np\n'), ((97, 14, 97, 35), 'numpy.dot', 'np.dot', ({(97, 21, 97, 25): 'Rot1', (97, 27, 97, 34): 'flWheel'}, {}), '(Rot1, flWheel)', True, 'import numpy as np\n'), ((99, 14, 99, 35), 'numpy.dot', 'np.dot', ({(99, 21, 99, 25): 'Rot1', (99, 27, 99, 34): 'rrWheel'}, {}), '(Rot1, rrWheel)', True, 'import numpy as np\n'), ((100, 14, 100, 35), 'numpy.dot', 'np.dot', ({(100, 21, 100, 25): 'Rot1', (100, 27, 100, 34): 'rlWheel'}, {}), '(Rot1, rlWheel)', True, 'import numpy as np\n'), ((101, 10, 101, 27), 'numpy.dot', 'np.dot', ({(101, 17, 101, 21): 'Rot1', (101, 23, 101, 26): 'car'}, {}), '(Rot1, car)', True, 'import numpy as np\n'), ((103, 15, 103, 35), 'numpy.array', 'np.array', ({(103, 24, 103, 34): '[[x], [y]]'}, {}), '([[x], [y]])', True, 'import numpy as np\n'), ((104, 15, 104, 35), 'numpy.array', 'np.array', ({(104, 24, 104, 34): '[[x], [y]]'}, {}), '([[x], [y]])', True, 'import numpy as np\n'), ((105, 15, 105, 35), 'numpy.array', 'np.array', ({(105, 24, 105, 34): '[[x], [y]]'}, {}), '([[x], [y]])', True, 'import numpy as np\n'), ((106, 15, 106, 35), 'numpy.array', 'np.array', ({(106, 24, 106, 34): '[[x], [y]]'}, {}), '([[x], [y]])', True, 'import numpy as np\n'), ((107, 11, 107, 31), 'numpy.array', 'np.array', ({(107, 20, 107, 30): '[[x], [y]]'}, {}), '([[x], [y]])', True, 'import numpy as np\n'), ((109, 4, 109, 41), 'matplotlib.pyplot.plot', 'plt.plot', ({(109, 13, 109, 22): 'car[(0), :]', (109, 24, 109, 33): 'car[(1), :]', (109, 35, 109, 40): 'color'}, {}), '(car[(0), :], car[(1), :], color)', True, 'import matplotlib.pyplot as plt\n'), ((110, 4, 110, 49), 'matplotlib.pyplot.plot', 'plt.plot', ({(110, 13, 110, 26): 'frWheel[(0), :]', (110, 28, 110, 41): 'frWheel[(1), :]', (110, 43, 110, 48): 'color'}, {}), '(frWheel[(0), :], frWheel[(1), :], color)', True, 'import matplotlib.pyplot as plt\n'), ((111, 4, 111, 49), 'matplotlib.pyplot.plot', 'plt.plot', ({(111, 13, 111, 26): 'rrWheel[(0), :]', (111, 28, 111, 41): 'rrWheel[(1), :]', (111, 43, 111, 48): 'color'}, {}), '(rrWheel[(0), :], rrWheel[(1), :], color)', True, 'import matplotlib.pyplot as plt\n'), ((112, 4, 112, 49), 'matplotlib.pyplot.plot', 'plt.plot', ({(112, 13, 112, 26): 'flWheel[(0), :]', (112, 28, 112, 41): 'flWheel[(1), :]', (112, 43, 112, 48): 'color'}, {}), '(flWheel[(0), :], flWheel[(1), :], color)', True, 'import matplotlib.pyplot as plt\n'), ((113, 4, 113, 49), 'matplotlib.pyplot.plot', 'plt.plot', ({(113, 13, 113, 26): 'rlWheel[(0), :]', (113, 28, 113, 41): 'rlWheel[(1), :]', (113, 43, 113, 48): 'color'}, {}), '(rlWheel[(0), :], rlWheel[(1), :], color)', True, 'import matplotlib.pyplot as plt\n'), ((11, 16, 11, 30), 'numpy.deg2rad', 'np.deg2rad', ({(11, 27, 11, 29): '30'}, {}), '(30)', True, 'import numpy as np\n'), ((31, 8, 31, 74), 'matplotlib.pyplot.plot', 'plt.plot', (), '', True, 'import matplotlib.pyplot as plt\n'), ((32, 8, 33, 66), 'matplotlib.pyplot.plot', 'plt.plot', (), '', True, 'import matplotlib.pyplot as plt\n'), ((34, 8, 35, 66), 'matplotlib.pyplot.plot', 'plt.plot', (), '', True, 'import matplotlib.pyplot as plt\n'), ((58, 8, 60, 44), 'matplotlib.pyplot.plot', 'plt.plot', (), '', True, 'import matplotlib.pyplot as plt\n'), ((68, 14, 69, 135), 'numpy.array', 'np.array', ({(68, 23, 69, 134): '[[-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]])', True, 'import numpy as np\n'), ((71, 14, 72, 89), 'numpy.array', 'np.array', ({(71, 23, 72, 88): '[[-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]])', True, 'import numpy as np\n'), ((17, 24, 17, 37), 'numpy.cos', 'np.cos', ({(17, 31, 17, 36): 'theta'}, {}), '(theta)', True, 'import numpy as np\n'), ((18, 24, 18, 37), 'numpy.sin', 'np.sin', ({(18, 31, 18, 36): 'theta'}, {}), '(theta)', True, 'import numpy as np\n'), ((24, 40, 24, 59), 'numpy.cos', 'np.cos', ({(24, 47, 24, 58): 'theta_hat_L'}, {}), '(theta_hat_L)', True, 'import numpy as np\n'), ((25, 40, 25, 59), 'numpy.cos', 'np.cos', ({(25, 47, 25, 58): 'theta_hat_R'}, {}), '(theta_hat_R)', True, 'import numpy as np\n'), ((28, 40, 28, 59), 'numpy.sin', 'np.sin', ({(28, 47, 28, 58): 'theta_hat_L'}, {}), '(theta_hat_L)', True, 'import numpy as np\n'), ((29, 40, 29, 59), 'numpy.sin', 'np.sin', ({(29, 47, 29, 58): 'theta_hat_R'}, {}), '(theta_hat_R)', True, 'import numpy as np\n'), ((42, 25, 42, 40), 'numpy.cos', 'np.cos', ({(42, 32, 42, 39): 'theta_B'}, {}), '(theta_B)', True, 'import numpy as np\n'), ((43, 25, 43, 40), 'numpy.sin', 'np.sin', ({(43, 32, 43, 39): 'theta_B'}, {}), '(theta_B)', True, 'import numpy as np\n'), ((48, 28, 48, 44), 'numpy.cos', 'np.cos', ({(48, 35, 48, 43): 'theta_BL'}, {}), '(theta_BL)', True, 'import numpy as np\n'), ((49, 28, 49, 44), 'numpy.sin', 'np.sin', ({(49, 35, 49, 43): 'theta_BL'}, {}), '(theta_BL)', True, 'import numpy as np\n'), ((50, 28, 50, 44), 'numpy.cos', 'np.cos', ({(50, 35, 50, 43): 'theta_BR'}, {}), '(theta_BR)', True, 'import numpy as np\n'), ((51, 28, 51, 44), 'numpy.sin', 'np.sin', ({(51, 35, 51, 43): 'theta_BR'}, {}), '(theta_BR)', True, 'import numpy as np\n'), ((53, 26, 53, 37), 'numpy.cos', 'np.cos', ({(53, 33, 53, 36): 'yaw'}, {}), '(yaw)', True, 'import numpy as np\n'), ((54, 26, 54, 37), 'numpy.sin', 'np.sin', ({(54, 33, 54, 36): 'yaw'}, {}), '(yaw)', True, 'import numpy as np\n'), ((55, 26, 55, 37), 'numpy.cos', 'np.cos', ({(55, 33, 55, 36): 'yaw'}, {}), '(yaw)', True, 'import numpy as np\n'), ((56, 26, 56, 37), 'numpy.sin', 'np.sin', ({(56, 33, 56, 36): 'yaw'}, {}), '(yaw)', True, 'import numpy as np\n'), ((82, 22, 82, 35), 'math.cos', 'math.cos', ({(82, 31, 82, 34): 'yaw'}, {}), '(yaw)', False, 'import math\n'), ((83, 22, 83, 35), 'math.sin', 'math.sin', ({(83, 31, 83, 34): 'yaw'}, {}), '(yaw)', False, 'import math\n'), ((83, 37, 83, 50), 'math.cos', 'math.cos', ({(83, 46, 83, 49): 'yaw'}, {}), '(yaw)', False, 'import math\n'), ((85, 22, 85, 37), 'math.cos', 'math.cos', ({(85, 31, 85, 36): 'steer'}, {}), '(steer)', False, 'import math\n'), ((85, 39, 85, 54), 'math.sin', 'math.sin', ({(85, 48, 85, 53): 'steer'}, {}), '(steer)', False, 'import math\n'), ((86, 40, 86, 55), 'math.cos', 'math.cos', ({(86, 49, 86, 54): 'steer'}, {}), '(steer)', False, 'import math\n'), ((82, 38, 82, 51), 'math.sin', 'math.sin', ({(82, 47, 82, 50): 'yaw'}, {}), '(yaw)', False, 'import math\n'), ((86, 23, 86, 38), 'math.sin', 'math.sin', ({(86, 32, 86, 37): 'steer'}, {}), '(steer)', False, 'import math\n')]