"""Classical control toy: linear algebra -> PID -> 2-link IK. Pure numpy. 2026-11-05."""
import numpy as np
np.set_printoptions(precision=3, suppress=True)

# 1) Matrices as rules
def R(t): return np.array([[np.cos(t), -np.sin(t)], [np.sin(t), np.cos(t)]])
S = np.diag([2.0, 0.5])
v = np.array([1.0, 0.0])
print("== 1. linear algebra")
print("R(90deg)@v =", R(np.pi/2) @ v)
print("S@R(90deg)@v =", S @ R(np.pi/2) @ v, " R@S@v =", R(np.pi/2) @ S @ v, "(order matters)")
print("det R =", round(np.linalg.det(R(0.7)), 6), " det S =", np.linalg.det(S))

# 2) PID on mass-spring-damper w/ constant disturbance: m x'' + c x' + k x = u + d
def sim(kp, ki, kd, T=30.0, dt=1e-3, m=1.0, c=0.5, k=1.0, d=-0.5, ref=1.0):
    x = v_ = integ = 0.0; prev_e = ref; xs = []
    for _ in range(int(T/dt)):
        e = ref - x; integ += e*dt; de = (e - prev_e)/dt; prev_e = e
        u = kp*e + ki*integ + kd*de
        a = (u + d - c*v_ - k*x)/m; v_ += a*dt; x += v_*dt; xs.append(x)
    xs = np.array(xs); t = np.arange(len(xs))*dt
    os_ = max(0.0, (xs.max()-ref)/ref*100); sse = ref - xs[-2000:].mean()
    out = np.where(np.abs(xs-ref) > 0.02*ref)[0]
    ts = t[out[-1]] if len(out) and out[-1] < len(xs)-1 else (np.nan if len(out) else 0.0)
    return os_, sse, ts
print("\n== 2. PID (plant m=1,c=0.5,k=1, disturbance d=-0.5, step ref=1, 30s)")
print(f"{'config':6} {'Kp':>4} {'Ki':>4} {'Kd':>4} {'overshoot%':>10} {'ss_err':>8} {'settle2%(s)':>11}")
for name,(kp,ki,kd) in {"P":(10,0,0),"PI":(10,5,0),"PD":(10,0,4),"PID":(10,5,4)}.items():
    o,e,ts = sim(kp,ki,kd)
    print(f"{name:6} {kp:4} {ki:4} {kd:4} {o:10.1f} {e:8.3f} {('>30' if np.isnan(ts) else f'{ts:.2f}'):>11}")

# 3) 2-link planar IK
l1, l2 = 1.0, 0.8
def fk(q): return np.array([l1*np.cos(q[0])+l2*np.cos(q[0]+q[1]), l1*np.sin(q[0])+l2*np.sin(q[0]+q[1])])
def jac(q):
    s1,c1,s12,c12 = np.sin(q[0]),np.cos(q[0]),np.sin(q[0]+q[1]),np.cos(q[0]+q[1])
    return np.array([[-l1*s1-l2*s12, -l2*s12],[l1*c1+l2*c12, l2*c12]])
def ik_analytic(p, elbow=+1):
    c2 = (p@p - l1**2 - l2**2)/(2*l1*l2)
    if abs(c2) > 1: return None
    q2 = elbow*np.arccos(c2)
    q1 = np.arctan2(p[1],p[0]) - np.arctan2(l2*np.sin(q2), l1+l2*np.cos(q2))
    return np.array([q1,q2])
def ik_dls(p, q0=np.array([0.3,0.3]), lam=0.1, iters=500, tol=1e-9):
    q = q0.copy()
    for i in range(iters):
        e = p - fk(q)
        if np.linalg.norm(e) < tol: return q, i, np.linalg.norm(e)
        J = jac(q); q = q + J.T @ np.linalg.solve(J@J.T + lam**2*np.eye(2), e)
    return q, iters, np.linalg.norm(p-fk(q))
print("\n== 3. IK (l1=1.0, l2=0.8)")
for p in [np.array([1.2,0.6]), np.array([0.3,1.4]), np.array([2.0,0.5])]:
    qa = ik_analytic(p)
    if qa is None: print(f"target {p}: unreachable (|p|={np.linalg.norm(p):.3f} > {l1+l2})")
    else: print(f"target {p}: analytic q={qa} err={np.linalg.norm(fk(qa)-p):.1e}; elbow-down q={ik_analytic(p,-1)}")
    q,it,err = ik_dls(p)
    print(f"   DLS iterative: q={q} iters={it} err={err:.1e}")
