from basic_kinematics import *
from source_utils import *
import random as rand
import sys
import math

xaxis = V3(1, 0, 0)
zaxis = V3(0, 0, 1)

def rotation_delta(r1, r2):
    "A measure of difference between two rotations"
    return min(abs(r1.quat - r2.quat), abs(r1.quat + r2.quat))

def test_axis_and_angle(eps):
    "Tests axis() and angle() functions of the Rotation class"
    r1 = random_rotation()
    r2 = Rotation(r1.axis(), r1.angle())
    assert rotation_delta(r1, r2) < eps

def test_ra_dec(eps):
    "Tests Rotation constructor from x, z right ascension and declination"
    r1 = random_rotation()
    x = r1(xaxis)
    z = r1(zaxis)
    r2 = Rotation(x.ra(), x.dec(), z.ra(), z.dec())
    assert rotation_delta(r1, r2) < eps

def test_slerp(eps):
    "Compares two forms of linear rotation interpolation"
    r1 = random_rotation()
    r2 = random_rotation()
    t = 2.0*rand.random() - 0.5
    i1 = slerp(r1, r2, t)
    i2 = slerp2(r1, r2, t)
    if (rotation_delta(i1, i2) > eps):
        print r1, r2, t
        assert 0

def test_scubic(eps):
    "Tests cubic rotation interpolation for consistency"
    r0 = random_rotation()
    r1 = random_rotation()
    r2 = random_rotation()
    r3 = random_rotation()
    s0 = scubic(r0, r1, r2, r3, 0.0)
    s1 = scubic(r0, r1, r2, r3, 1.0/3.0)
    s2 = scubic(r0, r1, r2, r3, 2.0/3.0)
    s3 = scubic(r0, r1, r2, r3, 1.0)
    assert rotation_delta(r0, s0) < eps
    assert rotation_delta(r1, s1) < eps
    assert rotation_delta(r2, s2) < eps
    assert rotation_delta(r3, s3) < eps

if __name__ == '__main__':
    tol = 1.0e-14
    ntries = 1000
    for i in xrange(ntries):
        test_axis_and_angle(tol)
        test_ra_dec(tol)
        test_slerp(tol)
        test_scubic(tol)
    print "OK"
    sys.exit(0)