import math
import matplotlib.patches
import matplotlib.pyplot
import numpy


def rotationmatrix(angle):
    s = math.sin(angle)
    c = math.cos(angle)
    return numpy.matrix([[c,-s],[s,c]])


class Car:
    def __init__(self, length, width, wheelbase, front_overhang, trackwidth, outer_turningradius):
        self.length = length
        self.width = width
        self.wheelbase = wheelbase
        self.front_overhang = front_overhang
        self.trackwidth = trackwidth
        self.outer_turningradius = outer_turningradius

    @property
    def rear_overhang(self):
        return self.length - self.wheelbase - self.front_overhang

    @property
    def innerturningradius(self):
        xo = self.wheelbase + self.front_overhang
        ro = self.outer_turningradius
        yo = math.sqrt(ro*ro - xo*xo)
        yi = yo - self.width
        return yi

    @property
    def boundingpoly(self):
        x0 = 0
        x1 = self.width
        y0 = self.rear_overhang
        y1 = -self.wheelbase - self.front_overhang
        return numpy.array([[x0, y0], [x1, y0], [x1, y1], [x0,y1]])


def paint_car(ax, car, pos, angle):
    poly = car.boundingpoly * rotationmatrix(angle) + pos
    ax.add_patch(matplotlib.patches.Polygon(
        poly, closed=True, facecolor='none', edgecolor='blue'))


def main():
    kuv100 = Car(3.700, 1.735, 2.385, 0.800, 1.490, 5.050)
    print('rear overhang: ', kuv100.rear_overhang)
    print('inner turning radius: ', kuv100.innerturningradius)
    fig, ax = matplotlib.pyplot.subplots(1, 1, subplot_kw={'aspect': 'equal'})
    ax.set_xlim(-4,6)
    ax.set_ylim(-5,5)
    ri = kuv100.innerturningradius
    print(ri)
    ax.plot(0,0,'go')
    for angle in range(-90, 1, 30):
        angle = math.radians(angle)
        x = ri * math.cos(angle)
        y = -ri * math.sin(angle)
        paint_car(ax, kuv100, (x,y), angle)
        ax.plot(x,y,'ro')
    matplotlib.pyplot.show()


if __name__ == '__main__':
    main()