import matplotlib.pyplot as plt
import descartes
import numpy as np
import math
from matplotlib.animation import FuncAnimation
import matplotlib.patches as patches
from shapely.geometry import Polygon

# vytvoření obrazu, nastavení os
fig = plt.figure('Animace')
ax = plt.axes(xlim=(0, 100), ylim=(0, 100))
plt.grid()

fig1 = plt.figure('Detekce')
fig1 = plt.axes()
fig1.set_xlim(0, 80)
fig1.set_ylim(0, 5000)

lenght_tire = 5
width_tire = 15
space_tire = 13
angle = 45
mass1 = 6250
mass2 = 5450


# funkce signum pro urceni kvadrantu pro vykresleni superelipsy
def sgn(x):
    return ((x > 0) - (x < 0)) * 1


# parametry, delka, sirka, polomery
a, b, r = lenght_tire / 2, width_tire / 2, 2.5
# vypocet exponentu pro zjednoduseni
ra = 2 / r
# vypocet presnosti (pocet bodu) superelipsy
step = 100
piece = (math.pi * 2) / step
pneu1 = []
pneu2 = []

# vypocet pocatecni pozice pneumatik
x1, y1 = 2, (width_tire / 2)
x2, y2 = 2, (y1 + width_tire + space_tire)

# vypocet bodu superelipse proti smeru hod. rucicek
t = 0
for i in range(step + 1):
    x = (abs((math.cos(t))) ** ra) * a * sgn(math.cos(t)) + x1
    y = (abs((math.sin(t))) ** ra) * b * sgn(math.sin(t)) + y1
    pneu1.append([x, y])

    x = (abs((math.cos(t))) ** ra) * a * sgn(math.cos(t)) + x2
    y = (abs((math.sin(t))) ** ra) * b * sgn(math.sin(t)) + y2
    pneu2.append([x, y])
    t = t + piece

# vytvoreni stop a pridani do animace
kolo1 = patches.Polygon(pneu1, closed=True, fc='y')
kolo2 = patches.Polygon(pneu2, closed=True, fc='b')
ax.add_patch(kolo1)
ax.add_patch(kolo2)

# vytvoreni senzoru
def sensor(point, angle, length, width):
    # načtení počátku
    x, y = point
    # vypocet bodu s sirkou
    x1 = x + width
    y1 = y
    # vypočet koncových bodů
    endy = y + length * math.sin(math.radians(angle))
    endx = x + length * math.cos(math.radians(angle))
    endx1 = x1 + length * math.cos(math.radians(angle))
    endy1 = y1 + length * math.sin(math.radians(angle))
    # vykresleni senzoru
    senzor = Polygon([(x, y), (x1, y1), (endx1, endy1), (endx, endy)])
    # zobrazení senzoru
    ax.add_patch(descartes.PolygonPatch(senzor))
    # rovnice přímky
    k = math.tan(math.radians(angle))
    q = -(x * k)
    q1 = -(x1 * k)
    return senzor

#funkce pro simulaci najezdu stop na senzor pod jinym uhlem
def motion(point, angle):
    x, y = point
    endymotion = 20 * math.sin(math.radians(angle)) / math.sin(math.radians(90 - angle)) + y
    return endymotion

#inicializacni funkce pro animaci
def init():
    return kolo1, kolo2,


def animate(i):
    pneu1 = kolo1.get_xy() + [0.1, 0]
    pneu2 = kolo2.get_xy() + [0.1, 0]
    kolo1.set_xy(pneu1)
    kolo2.set_xy(pneu2)

    rec_area1 = Polygon(pneu1)
    rec_area2 = Polygon(pneu2)
    full_area = rec_area1.area

    if (senzor.intersects(rec_area2)) and (senzor.intersects(rec_area1)) == True:
        inter2 = senzor.intersection(rec_area2)
        inter1 = senzor.intersection(rec_area1)
        area = (inter1.area + inter2.area) * (mass2/10)
        poly_area[i] = area
    elif senzor.intersects(rec_area1):
        inter1 = senzor.intersection(rec_area1)
        area = inter1.area
        poly_area[i] = area * (mass1/10)
    elif senzor.intersects(rec_area2):
        inter2 = senzor.intersection(rec_area2)
        area = inter2.area
        poly_area[i] = area * (mass2/10)


    poly_area_real = poly_area

    fig1 = plt.cla()
    fig1 = plt.grid()
    fig1 = plt.plot(x,poly_area_real, label = "Simulovaný signál")

    # naceteni realnych dat
    lines = np.loadtxt("signal_5_prah.dat", comments="#", delimiter=",", unpack=False)
    x_real = np.arange(0, len(lines), 1)
    fig1 = plt.plot(x_real,lines, label = "Reálný signál")
    fig1 = plt.legend()
    fig1 = plt.ylabel('ADC [-]')
    fig1 = plt.xlabel('t [ms]')

    #np.savetxt("pneu10_35.dat", poly_area_real, delimiter=",")
    #np.savetxt("pneu10_40.dat", poly_area_real, delimiter=",")
    #np.savetxt("pneu10_45.dat", poly_area_real, delimiter=",")
    #np.savetxt("pneu10_55.dat", poly_area_real, delimiter=",")
    #np.savetxt("pneu10_70.dat", poly_area_real, delimiter=",")

    return kolo1, kolo2,


# počátek, úhel, délka, šířka
senzor = sensor((10, 0), angle, 250, 2)
# počátek, úhel
endymotion = motion((2, 0), 0)
x = np.arange(0.0, 100, 0.1)
y = np.linspace(0, endymotion, num=len(x))
det = np.zeros(len(x))
poly_area = np.zeros(len(x))

# spusteni animace
anim = FuncAnimation(fig, animate, init_func=init, frames=len(x), interval=0.1, blit=True, repeat=False)

plt.grid()
plt.show()
