"""D-001 -- Euler-Lagrange verification (plain-script version of the notebook).
"""

from sympy import symbols, diff, simplify, sin, cos, sympify
from sympy.physics.mechanics import dynamicsymbols

# IMPORTANT: use dynamicsymbols' own time symbol, not a separately-created one.
# `symbols('t')` and `dynamicsymbols._t` print identically but are different
# objects -- differentiating with the wrong one silently returns zero instead
# of raising an error. This is the single most common trap in this derivation.
t = dynamicsymbols._t
m1, m2, l1, l2, g = symbols('m1 m2 l1 l2 g', positive=True)
theta1, theta2 = dynamicsymbols('theta1 theta2')
theta1d, theta2d = diff(theta1, t), diff(theta2, t)

# positions of the two bobs (angles measured from vertical)
x1 = l1*sin(theta1);            y1 = -l1*cos(theta1)
x2 = x1 + l2*sin(theta2);       y2 = y1 - l2*cos(theta2)
x1d, y1d = diff(x1, t), diff(y1, t)
x2d, y2d = diff(x2, t), diff(y2, t)

# Lagrangian: L = T - V
T = sympify(1)/2*m1*(x1d**2 + y1d**2) + sympify(1)/2*m2*(x2d**2 + y2d**2)
V = m1*g*y1 + m2*g*y2
L = T - V

def euler_lagrange(L, q):
    """d/dt(dL/dq_dot) - dL/dq, for a single generalized coordinate q."""
    qd = diff(q, t)
    return simplify(diff(diff(L, qd), t) - diff(L, q))

eom1 = euler_lagrange(L, theta1)   # generated equation of motion, coordinate 1
eom2 = euler_lagrange(L, theta2)   # generated equation of motion, coordinate 2

# standard textbook form of the double-pendulum equations of motion
theta1dd, theta2dd = diff(theta1, t, 2), diff(theta2, t, 2)

textbook_eom1 = ((m1+m2)*l1*theta1dd + m2*l2*theta2dd*cos(theta1-theta2)
                  + m2*l2*theta2d**2*sin(theta1-theta2) + (m1+m2)*g*sin(theta1))
textbook_eom2 = (l2*theta2dd + l1*theta1dd*cos(theta1-theta2)
                  - l1*theta1d**2*sin(theta1-theta2) + g*sin(theta2))

# The generated equations come out scaled by l1 and l2*m2 respectively --
# that's the natural units of "d/dt(dL/dq_dot) - dL/dq" for these coordinates,
# not an error. Compare like with like:
diff1 = simplify(eom1 - l1*textbook_eom1)
diff2 = simplify(eom2 - l2*m2*textbook_eom2)

assert diff1 == 0 and diff2 == 0, "generated equations do not match the textbook form"
print("PASS -- both generated equations match the textbook form exactly.")