"""Independent mechanical and finite-difference checks for chapter 11."""
import json
import math
import random
from efc import *

c = model_data()
s = contact_state(c['position'][2], c['radius'], c['margin'], c['gap'])
assert abs(s['distance']+.01) < 1e-14 and abs(s['error']+.03) < 1e-14
assert contact_state(.222, .2, .02, .005)['candidate']
assert not contact_state(.222, .2, .02, .005)['active']
# Evaluate exact boundary using binary-exact values.
assert not contact_state(.75, .5, .25, .125)['active']
assert contact_state(.875, .5, .25, .125)['candidate']
assert not contact_state(.876, .5, .25, .125)['candidate']
rng = random.Random(11)
power_error = force_error = curvature_error = 0.0
for dim in [1, 3, 4, 6]:
    for pyramid in [False, True]:
        J = contact_rows(dim, pyramid)
        assert len(J) == (2*(dim-1) if pyramid and dim > 1 else dim)
        for _ in range(80):
            v = [rng.uniform(-2, 2) for _ in range(3)]
            f = [rng.uniform(0, 4) for _ in J]
            tau = transpose_product(J, f)
            power_error = max(power_error, abs(dot(tau, v)-dot(f, matvec(J, v))))
            # Compare direct Newton accelerations with constraint-space response.
            A = inverse_inertia(J, [.5]*3)
            got = matvec(A, f)
            expected = matvec(J, [x/2 for x in tau])
            force_error = max(force_error, *(abs(a-b) for a, b in zip(got, expected)))
            if pyramid and dim > 1:
                physical = pyramid_force(f, [.6, .6, .01, .02, .02][:dim-1])
                assert max(abs(a-b) for a,b in zip(tau,transpose_product(contact_rows(dim),physical)))<1e-12
for speed in [-2, 0, .6, 3]:
    for t in [.1, .6, 1.1]:
        h = 1e-4
        # Geometric path gives r(t)=0. A tangent straight-line probe gives r''=u².
        p = [math.cos(speed*t), math.sin(speed*t)]
        v = [-speed*p[1], speed*p[0]]
        a = [-speed*speed*x for x in p]
        assert abs(dot(p,a)+dot(v,v))<1e-12
        r = lambda dt: .5*(sum((p[i]+v[i]*dt)**2 for i in range(2))-1)
        fd = (r(h)-2*r(0)+r(-h))/(h*h)
        curvature_error = max(curvature_error,abs(fd-dot(v,v)))
assert power_error<1e-12 and force_error<1e-12 and curvature_error<1e-6
print(json.dumps({'power_error':power_error,'newton_projection_error':force_error,
                  'curvature_finite_difference_error':curvature_error,'row_cases':640},indent=2))
# Finite-difference weld quaternion residual, independently advancing each body.
from quaternion import from_rotvec, step_world, conjugate, mul as qmul
weld_error = 0.0
for _ in range(150):
    q1,q2,q0 = [from_rotvec([rng.uniform(-2,2) for _ in range(3)]) for _ in range(3)]
    w1,w2 = [[rng.uniform(-2,2) for _ in range(3)] for _ in range(2)]
    h=1e-6
    residual = lambda dt: qmul(qmul(conjugate(step_world(q2,w2,dt)),step_world(q1,w1,dt)),q0)[1:]
    plus,minus=residual(h),residual(-h)
    got=[(a-b)/(2*h) for a,b in zip(plus,minus)]
    exact=[.5*x for x in qmul(qmul(qmul(conjugate(q2),[0]+[a-b for a,b in zip(w1,w2)]),q1),q0)[1:]]
    weld_error=max(weld_error,*(abs(a-b) for a,b in zip(got,exact)))
assert weld_error<1e-8
print(json.dumps({'weld_finite_difference_error':weld_error,'weld_cases':150},indent=2))