import numpy as np
import matplotlib.pyplot as plt

segments_length = [15, 50, 40, 30]
alpha = np.array([90, 0, 0, 0]) * (np.pi / 180)


def calculate_triangle_angles(a, b, c):
    # Calculate the angles using the law of cosines
    angle_a = np.rad2deg(np.arccos((b ** 2 + c ** 2 - a ** 2) / (2 * b * c)))
    angle_b = np.rad2deg(np.arccos((c ** 2 + a ** 2 - b ** 2) / (2 * c * a)))
    angle_c = 180 - angle_a - angle_b  # The sum of angles in a triangle is 180 degrees

    return angle_a, angle_b, angle_c


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
    print("relative:", relative_angels_stepper,
          "absolute:", absolute_angels)
    return absolute_angels


def calculate_angels_relative(absolute_angels_stepper):
    relative_angels = [0, 0, 0, 0]
    relative_angels[0] = absolute_angels_stepper[0]
    relative_angels[1] = absolute_angels_stepper[1] - relative_angels[0]
    relative_angels[2] = absolute_angels_stepper[2] - relative_angels[1]
    relative_angels[3] = absolute_angels_stepper[3] - relative_angels[2]

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

    return relative_angels


def ik_calculate_angels(g3_position, g4_orientation):
    z0 = segments_length[0]
    # y0 = 0

    alpha = [90, 0, 0, g4_orientation]  # relative stepper angels to reach desired coordination
    beta = [0, 0, 90]  # angels of right triangle

    y = g3_position[0]  # - y0
    z = g3_position[1] - z0

    l1 = segments_length[1]
    l2 = segments_length[2]
    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis

    gamma = calculate_triangle_angles(l2, l1, l_h)

    beta[0] = np.rad2deg(np.arctan2(z, y))
    beta[1] = np.rad2deg(np.arctan2(y, z))

    alpha[1] = -(90 - gamma[0] - beta[0])
    alpha[2] = -(180 - gamma[2])

    return alpha


# Wrong!!!!!
def fk_calculate_position(stepper_angels_rel):
    alpha = np.array(stepper_angels_rel) * (np.pi / 180)
    alpha_abs = calculate_angels_to_horizon(alpha)
    z = [0, 0, 0, 0]
    y = [0, 0, 0, 0]

    start_x = 0
    start_y = 0

    z[0] = segments_length[0]
    y[0] = 0

    z[1] = z[0]
    y[1] = y[0] + segments_length[1]

    z[2] = y[1] + np.cos(alpha_abs[2]) * segments_length[2]
    y[2] = z[1] + np.sin(alpha_abs[2]) * segments_length[2]

    z[3] = z[2] + np.cos(alpha_abs[3]) * segments_length[3]
    y[3] = y[2] + np.sin(alpha_abs[3]) * segments_length[3]

    m4_pos_2d = [y, z]
    m3_pos_2d = [y, z]

    return m4_pos_2d


default_stepper_angels = [90, -90, 0, 0]


# Definition of function to calculate stepper angels to move end effector to desired coordination by changing M2 and M3
def ik_calculate_angels2(end_effector_position):
    z0 = segments_length[0]
    y0 = segments_length[1]

    alpha = [90, -90, 0, 0]  # relative stepper angels to reach desired coordination
    beta = [0, 0, 90]  # angels of right triangle

    y = end_effector_position[0] - y0
    z = end_effector_position[1] - z0

    l2 = segments_length[2]
    l3 = segments_length[3]
    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis

    gamma = calculate_triangle_angles(l3, l2, l_h)

    beta[0] = np.rad2deg(np.arctan2(z, y))
    beta[1] = np.rad2deg(np.arctan2(y, z))

    alpha[2] = gamma[0] + beta[0]
    alpha[3] = -(180 - gamma[2])

    return alpha


pos = [40, 30]
alpha_degree = ik_calculate_angels2(pos)
alpha_rel = np.array(alpha_degree)
alpha_abs = np.array(calculate_angels_to_horizon(alpha_rel)) * (np.pi / 180)

start_x = 0
start_y = 0

x0 = np.cos(alpha_abs[0]) * segments_length[0]
y0 = np.sin(alpha_abs[0]) * segments_length[0]

x1 = x0 + np.cos(alpha_abs[1]) * segments_length[1]
y1 = y0 + np.sin(alpha_abs[1]) * segments_length[1]

x2 = x1 + np.cos(alpha_abs[2]) * segments_length[2]
y2 = y1 + np.sin(alpha_abs[2]) * segments_length[2]

x3 = x2 + np.cos(alpha_abs[3]) * segments_length[3]
y3 = y2 + np.sin(alpha_abs[3]) * segments_length[3]

# dev_x = [start_x, fk_calculate_position(alpha_degree)[0][0], fk_calculate_position(alpha_degree)[0][1],
# fk_calculate_position(alpha_degree)[0][2],
# fk_calculate_position(alpha_degree)[0][3]]
# dev_y = [start_y, fk_calculate_position(alpha_degree)[1][0], fk_calculate_position(alpha_degree)[1][1],
# fk_calculate_position(alpha_degree)[1][2],
# fk_calculate_position(alpha_degree)[1][3]]

dev_x = [start_x, x0, x1, x2, x3]
dev_y = [start_y, y0, y1, y2, y3]

# Create a figure and axis
# fig, ax = plt.subplots()

# Set the limits for the x and y axes
# ax.set_xlim(-130, 130)
# ax.set_ylim(-130, 130)

plt.plot(dev_x, dev_y, marker='o')
plt.grid()
plt.xlabel('Y in cm')
plt.ylabel('Z in cm')

plt.show()