#!/usr/bin/env python3
"""Deterministic basis checks for the local/world gravity convention used by test11."""
import math


def q_axis(axis, degrees):
    a = math.radians(degrees) * 0.5
    s = math.sin(a)
    return (axis[0]*s, axis[1]*s, axis[2]*s, math.cos(a))


def q_conj(q):
    return (-q[0], -q[1], -q[2], q[3])


def q_mul(a, b):
    ax, ay, az, aw = a; bx, by, bz, bw = b
    return (
        aw*bx + ax*bw + ay*bz - az*by,
        aw*by - ax*bz + ay*bw + az*bx,
        aw*bz + ax*by - ay*bx + az*bw,
        aw*bw - ax*bx - ay*by - az*bz,
    )


def rotate(q, v):
    r = q_mul(q_mul(q, (v[0], v[1], v[2], 0.0)), q_conj(q))
    return r[:3]


def close(a, b, eps=1e-9):
    return all(abs(x-y) <= eps for x, y in zip(a, b))


UP = (0.0, 1.0, 0.0)
DOWN = (0.0, -1.0, 0.0)
for angle in (0, 20, 90, 180):
    q = q_axis((1.0, 0.0, 0.0), angle)
    world_up = rotate(q, UP)
    world_gravity = rotate(q, DOWN)
    assert close(rotate(q_conj(q), world_up), UP)
    assert close(rotate(q_conj(q), world_gravity), DOWN)
    assert close(tuple(-x for x in world_up), world_gravity)
    print(f"X {angle:3d}: up={world_up!r}, gravity={world_gravity!r}")

q180 = q_axis((1.0, 0.0, 0.0), 180)
assert close(rotate(q180, UP), (0.0, -1.0, 0.0))
assert close(rotate(q180, DOWN), (0.0, 1.0, 0.0))
print("OK: at 180 degrees jump points world-down and gravity points world-up")