import matplotlib.pyplot as plt
import numpy as np


n = 100000

l = [0, 50, 40, 30]

alpha_0 = np.ones(n) * (90+90)
alpha_1 = np.random.uniform(-115+90, 115+90, n)
alpha_2 = np.random.uniform(-115+90, 115+90, n)
alpha_3 = np.random.uniform(-115+90, 115+90, n)


def calculate_angels_to_horizon(relative_angels_stepper):
    absolute_angels = [0, 0, 0, 0]
    absolute_angels[0] = relative_angels_stepper[0]
    absolute_angels[1] = absolute_angels[0] + relative_angels_stepper[1]
    absolute_angels[2] = absolute_angels[1] + relative_angels_stepper[2]
    absolute_angels[3] = absolute_angels[2] + relative_angels_stepper[3]

    for j in range(4):
        if absolute_angels[j] > 360:
            absolute_angels[j] = absolute_angels[j] - 360

    return absolute_angels


alpha_rel = np.vstack((alpha_0, alpha_1, alpha_2, alpha_3)) * (np.pi / 180)

print(calculate_angels_to_horizon([90, -115, -115, -115]))
print(np.sin(np.pi*3/2))

start_x = 0
start_y = 0

x0 = np.zeros(n)
y0 = np.zeros(n)
x1 = np.zeros(n)
y1 = np.zeros(n)
x2 = np.zeros(n)
y2 = np.zeros(n)
x3 = np.zeros(n)
y3 = np.zeros(n)

for dim in range(n):
    alpha = calculate_angels_to_horizon(
        calculate_angels_to_horizon([alpha_rel[0, dim], alpha_rel[1, dim], alpha_rel[2, dim], alpha_rel[3, dim]]))

    x0[dim] = start_x + np.cos(alpha[0]) * l[0]
    y0[dim] = start_y + np.sin(alpha[0]) * l[0]

    x1[dim] = x0[dim] + np.cos(alpha[1]) * l[1]
    y1[dim] = y0[dim] + np.sin(alpha[1]) * l[1]

    x2[dim] = x1[dim] + np.cos(alpha[2]) * l[2]
    y2[dim] = y1[dim] + np.sin(alpha[2]) * l[2]

    x3[dim] = x2[dim] + np.cos(alpha[3]) * l[3]
    y3[dim] = y2[dim] + np.sin(alpha[3]) * l[3]

plt.scatter(x3, y3, marker='x', color='red')
plt.xlabel('Y in cm')
plt.ylabel('X in cm')
plt.title('Movement')
plt.grid()

plt.show()