import legrandchien as lgc
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import numpy as np

g = lgc.Const(-10)
m = lgc.Const(1)
r = lgc.Const(1)

theta = lgc.Var("theta")

x = r * lgc.cos(theta)
y = r * lgc.sin(theta)

T = 0.5 * m * (x.diff()*x.diff() + y.diff()*y.diff())
V = m * g * r * lgc.cos(theta)

L = T - V

init = {
    theta : [-3 * 3.14/4, 0], # theta, theta.diff()
}

dt = 0.01
data = L.solve(init, 10, dt, True)

# Extraire les positions
times = sorted(data.keys())
positions = [data[t] for t in times]

x_positions = [r.value * np.sin(p["theta"]) for p in positions]
y_positions = [r.value * np.cos(p["theta"]) for p in positions]

# Calculer l'intervalle de frame pour 25 fps
fps = 25
frame_skip = max(1, int(1 / (fps * dt)))

# Filtrer les frames
display_indices = list(range(0, len(x_positions), frame_skip))
x_display = [x_positions[i] for i in display_indices]
y_display = [y_positions[i] for i in display_indices]

# Créer la figure et l'axe
fig, ax = plt.subplots(figsize=(8, 8))
ax.set_xlim(-3, 3)
ax.set_ylim(-3, 3)
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.grid(True)
ax.invert_yaxis()

line, = ax.plot([], [], 'o-', lw=2, markersize=8, label='Pendule')
trace, = ax.plot([], [], 'r-', alpha=0.3, lw=1, label='Trace')

def animate(frame):
    line.set_data([0, x_display[frame]], [0, y_display[frame]])
    trace.set_data(x_display[:frame+1], y_display[:frame+1])
    return line, trace

ax.legend()
anim = animation.FuncAnimation(fig, animate, frames=len(x_display), interval=40, blit=True, repeat=True)

plt.show()