import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from scipy.optimize import fsolve

# Link lengths (arbitrary units)
L1 = 4.0  # Ground link
L2 = 2.0  # Input crank
L3 = 3.0  # Coupler
L4 = 3.5  # Output rocker

# Fixed pivots
A = np.array([0, 0])
D = np.array([L1, 0])

# Input crank angle range
theta2_vals = np.linspace(0, 2 * np.pi, 360)

# Store previous angles to ensure continuity
previous_guess = [np.pi / 2, np.pi / 2]

# Solve joint positions using loop closure equations
def solve_positions(theta2):
    global previous_guess
    B = A + L2 * np.array([np.cos(theta2), np.sin(theta2)])

    def loop_closure(vars):
        theta3, theta4 = vars
        eq1 = L2 * np.cos(theta2) + L3 * np.cos(theta3) - L4 * np.cos(theta4) - L1
        eq2 = L2 * np.sin(theta2) + L3 * np.sin(theta3) - L4 * np.sin(theta4)
        return [eq1, eq2]

    theta3, theta4 = fsolve(loop_closure, previous_guess)
    previous_guess = [theta3, theta4]  # update guess for continuity

    C = B + L3 * np.array([np.cos(theta3), np.sin(theta3)])
    return A, B, C, D

# Set up plot
fig, ax = plt.subplots()
ax.set_xlim(-5, 8)
ax.set_ylim(-5, 5)
ax.set_aspect('equal')
line, = ax.plot([], [], 'o-', lw=2)

def init():
    line.set_data([], [])
    return line,

def update(frame):
    A, B, C, D = solve_positions(theta2_vals[frame])
    x_vals = [A[0], B[0], C[0], D[0]]
    y_vals = [A[1], B[1], C[1], D[1]]
    line.set_data(x_vals, y_vals)
    return line,

ani = animation.FuncAnimation(fig, update, frames=len(theta2_vals), init_func=init,
                              blit=True, interval=20, repeat=True)
plt.title("Four-Bar Linkage Simulation")
plt.show()