#!/usr/bin/env python3
"""Regression checks for local creative-flight input and changing ship orientation."""
import math


def qx(deg):
    a = math.radians(deg) / 2.0
    return (math.cos(a), math.sin(a), 0.0, 0.0)  # w,x,y,z


def conj(q):
    w,x,y,z=q
    return (w,-x,-y,-z)


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


def rotate(q,v):
    p=(0.0,*v)
    r=mul(mul(q,p),conj(q))
    return r[1:]


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

# Stored local velocity is authoritative. LocalPlayer.aiStep adds a local-Y flight impulse,
# already rotated into world space by Sable. Reconciliation must recover exactly that impulse,
# even when the frame orientation changes between ticks.
stored_local=(0.13,-0.04,0.27)
ascend_local=(0.0,0.15,0.0)
for previous,current in ((0,20),(20,90),(90,180),(180,10)):
    q=qx(current)
    expected_world=rotate(q,stored_local)
    actual_world=add(expected_world,rotate(q,ascend_local))
    external_world=sub(actual_world,expected_world)
    recovered_external=rotate(conj(q),external_world)
    reconciled=add(stored_local,recovered_external)
    assert close(recovered_external,ascend_local)
    assert close(reconciled,add(stored_local,ascend_local))
    print(f"{previous:3d}->{current:3d}: recovered flight impulse {recovered_external}")

# A frame rotation by itself must not manufacture a flight impulse.
for deg in (0,20,90,180):
    q=qx(deg)
    expected=rotate(q,stored_local)
    recovered=rotate(conj(q),sub(expected,expected))
    assert close(recovered,(0.0,0.0,0.0))

print('OK: flight input is recovered in local Y and frame rotation adds no fake acceleration')