use Test::More tests => 77; 
use Math::Trig;
use strict;
use Carp;
use Math::Quaternion;


# First, check that we can load the module...

BEGIN { use_ok("Math::Quaternion"); }

my $epsilon = 1e-10; # Precision to which I can be bothered with worrying.
my $pi = 3.1459265358979323846;

sub equal_fuzz {
	croak("Wrong number of args") unless (2==@_);
	my ($a,$b)=@_;
	if (0==$a) {
		if (abs($b)<$epsilon) {
			return 1;
		} else { 
			return undef;
		}
	}
	if (0==$b) {
		if (abs($a)<$epsilon) {
			return 1;
		} else { 
			return undef;
		}
	}
	if (abs(($a-$b)/$a) < $epsilon) {
		return 1;
	} else {
		return undef;
	}
}


# Take 5 args: a quat and four numbers. Return 1 if the quat is really a quat,
# and equal to the four numbers.

sub checkquat {
	croak("Wrong number of args") unless (5==@_);
	my ($q,@nos) = @_;
	if ("Math::Quaternion" ne ref $q) {
		return undef;
	}

	if (
		    equal_fuzz ($q->[0] , $nos[0])
		 && equal_fuzz ($q->[1] , $nos[1])
		 && equal_fuzz ($q->[2] , $nos[2])
		 && equal_fuzz ($q->[3] , $nos[3])
	) {
		return 1;
	} else { 
		return undef;
	}
}

sub quatequal_fuzz {
	my ($q1,$q2) = @_;

	if (
		    equal_fuzz ($q1->[0] , $q2->[0])
		 && equal_fuzz ($q1->[1] , $q2->[1])
		 && equal_fuzz ($q1->[2] , $q2->[2])
		 && equal_fuzz ($q1->[3] , $q2->[3])
	) {
		return 1;
	} else {
		return undef;
	}

}

# Build a random unit vector in R^3
sub random_unitvector {
	my $theta = rand($pi);
	my $phi = rand(2.0*$pi);
	my $ax = sin($theta) * cos($phi);
	my $ay = sin($theta) * sin($phi);
	my $az = cos($theta);

	return ($ax,$ay,$az);
}

# Test constructors.

my $q=undef;

ok( ref($q = Math::Quaternion->new) eq "Math::Quaternion",
	"Math::Quaternion->new() makes a new quaternion..." );

ok( checkquat($q,1,0,0,0), "...and it's a unit quaternion by default.");

my ($a,$b,$c,$d) = map {rand} 1..4;
my ($e,$f,$g,$h) = map {rand} 1..4;
my @randoms = map {rand} 1..4;

my $q1 = Math::Quaternion->new($a,$b,$c,$d);
my $q2 = Math::Quaternion->new($e,$f,$g,$h);
my $q3 = Math::Quaternion->new(@randoms);


ok(	checkquat(	$q = Math::Quaternion->new(1,2,3,4),
			1,2,3,4),
	"Math::Quaternion->new(1,2,3,4) builds the correct quaternion.");

ok(	checkquat(Math::Quaternion->new($q),1,2,3,4),
	"Math::Quaternion->new(\$q) copies quaternion \$q");

ok(	checkquat(Math::Quaternion->new([5,6,7,8]),5,6,7,8),
	"Math::Quaternion->new([5,6,7,8]) builds the correct quaternion.");

ok(	checkquat(Math::Quaternion->new($a),$a,0,0,0),
	"Math::Quaternion->new(\$a) builds a scalar quaternion");

ok(	checkquat(Math::Quaternion->new($a,$b,$c),0,$a,$b,$c),
	"Math::Quaternion->new(\$a,\$b,\$c) builds a vector quaternion");


ok(	equal_fuzz(Math::Quaternion::dot($q1,$q2),
		$a*$e+$b*$f+$c*$g+$d*$h),
	"Math::Quaternion::dot(\$q1,\$q2) gives a dot product");



eval { Math::Quaternion->new(1,2) };

ok(	$@,"Math::Quaternion->new(1,2) fails.");

ok(	checkquat(Math::Quaternion->unit, 1,0,0,0),
	"Math::Quaternion->unit() builds a new unit quaternion");

ok(	checkquat(Math::Quaternion::conjugate($q),1,-2,-3,-4),
	"Math::Quaternion::conjugate() conjugates");
ok(	checkquat($q,1,2,3,4),
	"Math::Quaternion::conjugate leaves argument unchanged.");

ok( checkquat(Math::Quaternion::negate($q1),
	-($q1->[0]), -($q1->[1]), -($q1->[2]), -($q1->[3])),
	"Math::Quaternion::negate() negates");

ok(	checkquat( Math::Quaternion::plus($q1,$q2), $a+$e, $b+$f, $c+$g, $d+$h),
	"Math::Quaternion::plus() adds.");
ok(	checkquat( Math::Quaternion::plus($q2,$q1), $a+$e, $b+$f, $c+$g, $d+$h),
	"Math::Quaternion::plus() commutes.");
ok(	checkquat( Math::Quaternion::minus($q1,$q2), $a-$e, $b-$f, $c-$g, $d-$h),
	"Math::Quaternion::minus() subtracts.");

ok(	Math::Quaternion::squarednorm($q1) == $a*$a+$b*$b+$c*$c+$d*$d,
	"Math::Quaternion::squarednorm(\$q) returns squared norm of \$q");

ok(	checkquat(Math::Quaternion::scale($q1,$e),$a*$e,$b*$e,$c*$e,$d*$e),
	"Math::Quaternion::scale() performs scalar multiplication");

my $q1c = Math::Quaternion::conjugate($q1);
my $q1i = Math::Quaternion::inverse($q1);

ok(	checkquat(Math::Quaternion::multiply($q1,$q1c),
		Math::Quaternion::squarednorm($q1),0,0,0),
	"Multiplying with a conjugate gives the squared norm");

ok(	checkquat(Math::Quaternion::multiply($q1,$q1i),
		1,0,0,0),
	"Multiplying with inverse gives unit quaternion");


ok(	quatequal_fuzz(
		Math::Quaternion::multiply(
			$q1,
			Math::Quaternion::plus($q2,$q3)
		),
		Math::Quaternion::plus(
			Math::Quaternion::multiply($q1,$q2),
			Math::Quaternion::multiply($q1,$q3)
		)
	),
	"Quaternion multiplication is left-distributive");

ok(	quatequal_fuzz(
		Math::Quaternion::multiply(
			Math::Quaternion::plus($q1,$q2),
			$q3,
		),
		Math::Quaternion::plus(
			Math::Quaternion::multiply($q1,$q3),
			Math::Quaternion::multiply($q2,$q3)
		)
	),
	"Quaternion multiplication is right-distributive");

ok(	quatequal_fuzz(
		Math::Quaternion::multiply($q1,Math::Quaternion::multiply($q2,$q3)),
		Math::Quaternion::multiply(Math::Quaternion::multiply($q1,$q2),$q3),

	),
	"Quaternion multiplication is associative");

my $q1q2 = Math::Quaternion::multiply($q1,$q2);

my ($a0,$a1,$a2,$a3) = @$q1;
my ($b0,$b1,$b2,$b3) = @$q2;

ok(	checkquat($q1q2,
		$a0*$b0 - $a1*$b1 - $a2*$b2 - $a3*$b3,
		$a0*$b1 + $b0*$a1 + $a2*$b3 - $a3*$b2,
		$a0*$b2 + $b0*$a2 + $a3*$b1 - $a1*$b3,
		$a0*$b3 + $b0*$a3 + $a1*$b2 - $a2*$b1
	),
	"Math::Quaternion::multiply multiplies.");

ok(	equal_fuzz(1.0,Math::Quaternion::squarednorm(Math::Quaternion::normalize($q1))),
	"Math::Quaternion::normalize() produces a unit quaternion");

ok(	quatequal_fuzz(
		Math::Quaternion::inverse(Math::Quaternion::normalize($q1)),
		Math::Quaternion::conjugate(Math::Quaternion::normalize($q1))),
	"The inverse of a unit quaternion is its conjugate");

my $exponent = int rand 100;
my $q1manual = $q1;
for (1..($exponent-1))  { $q1manual = Math::Quaternion::multiply($q1,$q1manual); }
my $q1expon  = Math::Quaternion::power($q1,$exponent);

ok( quatequal_fuzz($q1manual,$q1expon),
	"Quaternions can be raised to integer exponents");

$exponent = rand 10;
$q1expon = Math::Quaternion::power($q1,$exponent);
ok( equal_fuzz($q1expon->modulus,($q1->modulus)**$exponent),
	"Quaternion powers raise magnitude to same power");

ok( quatequal_fuzz($q1i,Math::Quaternion::power($q1,-1)),
	"Quaternion raised to (-1)th power is inverse");


# Check exponentiation

{
	my ($ux,$uy,$uz) = random_unitvector();
	my $x = rand;
	my $y = rand;
	my $z = rand;
	my $theta = rand;
	my $q = Math::Quaternion->new($x,$y*$ux,$y*$uy,$y*$uz);
	my $etox = exp($x);
	my $cy = cos($y);
	my $yc = $etox * sin($y);
	# exp(q) for q=(r,0,0,0) should give (exp(r),0,0,0)
	ok(checkquat(Math::Quaternion::exp(Math::Quaternion->new($z)),exp($z),0,0,0),
		"Exponentiation of real quaternion works");
	# exp(ut) for unit u, scalar t should give
	# ( cos(t), u*sin(t) )
	ok(checkquat(
	 Math::Quaternion::exp(Math::Quaternion->new($theta*$ux,$theta*$uy,$theta*$uz)),
	 cos($theta),$ux*sin($theta),$uy*sin($theta),$uz*sin($theta)),
	 	"Exponentiation of pure quaternion works");
		
	# exp(x+uy) = exp(x) ( cos(y) + u sin(y) )
	ok(checkquat( Math::Quaternion::exp($q), $etox*$cy,$yc*$ux,$yc*$uy,$yc*$uz),
		"Quaternion exponentiation exponentiates");


# Check logarithms

	ok(checkquat(Math::Quaternion::log(Math::Quaternion->new($x)),log($x),0,0,0),
		"Logarithm of real quaternion works");
	
	my $explogq = Math::Quaternion::exp(Math::Quaternion::log($q1));
	my $logexpq = Math::Quaternion::log(Math::Quaternion::exp($q1));

	ok(quatequal_fuzz($explogq,$logexpq),
		"Exponentiation and Logarithm are mutually inverse");
	ok(quatequal_fuzz(Math::Quaternion::log($q1),Math::Quaternion::log([$a0,$a1,$a2,$a3])),
		"Math::Quaternion::log([a,b,c,d]) works");
	
}

{
	my $ql = Math::Quaternion->new(-2,0,0,0);
	my $logq = Math::Quaternion::log($ql);
	ok( checkquat($logq,
		CORE::log(2),pi,0,0),
		"log( (-2,0,0,0) ) = (log(2),pi,0,0)");
}


# Check that exp(q) is the same as e**q

ok(quatequal_fuzz(
	Math::Quaternion::power(exp(1),$q1),
	Math::Quaternion::exp($q1)),
	"Scalar exp(1) raised to quaternion power is the same as exponentiation"
	);

	

my $s = rand; # Random scalar

ok(quatequal_fuzz(Math::Quaternion::power($s,$q2),
	Math::Quaternion::exp(Math::Quaternion::multiply($q2,log($s)))),
	"a**b == exp(b*log(a)) for scalar a, quaternion b");

ok(quatequal_fuzz(Math::Quaternion::power($q1,$s),
	Math::Quaternion::exp(Math::Quaternion::multiply($s,Math::Quaternion::log($q1)))),
	"a**b == exp(b*log(a)) for quaternion a, scalar b");

ok(quatequal_fuzz(Math::Quaternion::power($q1,$q2),
	Math::Quaternion::exp(Math::Quaternion::multiply($q2,Math::Quaternion::log($q1)))),
	"a**b == exp(b*log(a)) for quaternion a,b");

# Build a random unit vector in R^3.

my ($ax,$ay,$az) = random_unitvector();
my ($bx,$by,$bz) = random_unitvector();

ok( equal_fuzz(1,sqrt($ax*$ax+$ay*$ay+$az*$az)),
	"Sanity check: I can make a random unit vector properly");

my $theta = rand(0.25*$pi);
my $rotquat = undef;
my $rq2 = undef;

ok( $rotquat = Math::Quaternion::rotation($theta,$ax,$ay,$az),
	"Math::Quaternion::rotation does not fail");
ok( equal_fuzz($rotquat->rotation_angle , $theta),
	"rotation_angle works");
ok( $rq2 = Math::Quaternion->new({axis => [$ax,$ay,$az],angle=>$theta}),
	"Math::Quaternion->new(\\\%hash) does not fail");
ok( quatequal_fuzz($rotquat,$rq2),
	"Math::Quaternion->new(\\\%hash) produces correct rotation");

ok(!defined( eval { Math::Quaternion::rotation(23,"skidoo"); 1; }),
	"Math::Quaternion::rotation(rubbish) fails");
ok(!defined( eval { Math::Quaternion::rotation(23,"skidoo",3,4,5); 1; }),
	"Math::Quaternion::rotation(much rubbish) fails");

my @vec = ($ax,$ay,$az);
my $rq2 = Math::Quaternion::rotation($theta,\@vec);
ok(quatequal_fuzz($rotquat,$rq2),"Math::Quaternion::rotation(\$theta,\\\@vec) works");
$rq2 = Math::Quaternion::rotation(\@vec,$theta);
ok(quatequal_fuzz($rotquat,$rq2),"Math::Quaternion::rotation(\\\@vec,\$theta) works");

my ($rax,$ray,$raz);
($rax,$ray,$raz) = $rotquat->rotation_axis;

ok( equal_fuzz($rax,$ax) && equal_fuzz($ray,$ay) && equal_fuzz($raz,$az),
	"rotation_axis works");
{
	my $qnull = Math::Quaternion->new(1,0,0,0);
	my ($zx,$zy,$zz) = $qnull->rotation_axis;
	ok(equal_fuzz(0,$zx) && equal_fuzz(0,$zy) && equal_fuzz($zz,1),
		"(1,0,0,0)->rotation_axis() returns Z axis rather than crashing");

}

my ($x,$y,$z) = map {rand} 1..3;
my @r=$rotquat->rotate_vector($x,$y,$z);
ok( 3==@r, "Math::Quaternion::rotate_vector() produces a 3-vector");

my ($xx,$yy,$zz) = @r;
my $m1 = sqrt($x*$x+$y*$y+$z*$z);
my $m2 = sqrt($xx*$xx + $yy*$yy + $zz*$zz);

ok( equal_fuzz($m1,$m2), "Rotation preserves length");

# Let v and a be vectors. The component of v parallel to a is
# |v| cos theta = (v.a)/|a|, or (v.a) ahat, in vector form.
# Hence, the component of v perp to a is v - (v.a) ahat ; if
# a is a unit vector, then it's just v - (v.a) a .
# Let v' be v rotated by angle phi about a. Let
# u' and u be the components of v' and v perpendicular to a.
# Then u.u' == |u| |u'| cos phi == |u|^2 cos phi

my $vdota = $x*$ax + $y*$ay + $z*$az;
my ($ux,$uy,$uz) =    ($x -$vdota*$ax, $y -$vdota*$ay, $z -$vdota*$az);
my ($uxx,$uyy,$uzz) = ($xx-$vdota*$ax, $yy-$vdota*$ay, $zz-$vdota*$az);

my $dotproduct = $ux*$uxx + $uy*$uyy + $uz*$uzz;
my $modu = sqrt($ux*$ux+$uy*$uy+$uz*$uz);
my $moduu = sqrt($uxx*$uxx+$uyy*$uyy+$uzz*$uzz);

ok( equal_fuzz($modu,$moduu),
"Rotation preserves component perpendicular to axes");

ok( equal_fuzz($dotproduct,$modu*$moduu*cos($theta)),
	"Rotated vector makes correct dot product with original");



my @m;

ok( @m = $rotquat->matrix4x4, "matrix4x4() doesn't fail...");
ok( 16==@m, "...and produces a 4x4 matrix...");

my $mx = $m[ 0]*$x + $m[ 4]*$y + $m[ 8]*$z;
my $my = $m[ 1]*$x + $m[ 5]*$y + $m[ 9]*$z;
my $mz = $m[ 2]*$x + $m[ 6]*$y + $m[10]*$z;

ok( equal_fuzz($mx,$xx) && equal_fuzz($my,$yy) && equal_fuzz($mz,$zz),
	"...which produces the same rotation as the quaternion.");

my ($mr,$mir) = $rotquat->matrix4x4andinverse;
my @mr = @$mr; my @mir = @$mir;

$mx = $mr[ 0]*$x + $mr[ 4]*$y + $mr[ 8]*$z;
$my = $mr[ 1]*$x + $mr[ 5]*$y + $mr[ 9]*$z;
$mz = $mr[ 2]*$x + $mr[ 6]*$y + $mr[10]*$z;

ok( equal_fuzz($mx,$xx) && equal_fuzz($my,$yy) && equal_fuzz($mz,$zz),
	"matrix4x4andinverse() produces the same rotation matrix...");

my $mmx = $mir[ 0]*$mx + $mir[ 4]*$my + $mir[ 8]*$mz;
my $mmy = $mir[ 1]*$mx + $mir[ 5]*$my + $mir[ 9]*$mz;
my $mmz = $mir[ 2]*$mx + $mir[ 6]*$my + $mir[10]*$mz;

ok( equal_fuzz($mmx,$x) && equal_fuzz($mmy,$y) && equal_fuzz($mmz,$z),
	"...as well as its inverse.");

ok( @m = $rotquat->matrix3x3, "matrix3x3() doesn't fail...");
ok( 9==@m, "...and produces a 3x3 matrix...");

$mx = $m[ 0]*$x + $m[ 3]*$y + $m[ 6]*$z;
$my = $m[ 1]*$x + $m[ 4]*$y + $m[ 7]*$z;
$mz = $m[ 2]*$x + $m[ 5]*$y + $m[ 8]*$z;

ok( equal_fuzz($mx,$xx) && equal_fuzz($my,$yy) && equal_fuzz($mz,$zz),
	"...which produces the same rotation as the quaternion.");

my @v1=($ax,$ay,$az);
my @v2=($bx,$by,$bz);
ok($rotquat = Math::Quaternion::rotation(\@v1,\@v2),
	"Math::Quaternion::rotation(\\\@v1,\\\@v2) does not fail");

ok($rq2 = Math::Quaternion->new({ 'v1'=>\@v1, 'v2'=>\@v2 }),
	"Math::Quaternion->new({v1=>..,v2=>..}) does not fail");
ok(quatequal_fuzz($rotquat,$rq2),
	"Math::Quaternion->new({v1=>..,v2=>..}) produces correct quaternion");
ok(!defined( eval { Math::Quaternion->new({badger=>1,wibble=>7}); 1; }),
	"Math::Quaternion->new(rubbish) fails");

my ($cx,$cy,$cz) = $rotquat->rotation_axis;
ok(
	equal_fuzz(0,$ax*$cx+$ay*$cy+$az*$cz) &&
	equal_fuzz(0,$bx*$cx+$by*$cy+$bz*$cz) ,
	"Rotation axis perpendicular to start and end orientations");

my $dotprod = $ax*$bx+$ay*$by+$az*$bz;
my $moda = sqrt($ax*$ax+$ay*$ay+$az*$az);
my $modb = sqrt($bx*$bx+$by*$by+$bz*$bz);
ok( equal_fuzz($dotprod,$moda*$modb*cos($rotquat->rotation_angle)),
	"Rotation angle is angle between start and end vectors");

my ($dx,$dy,$dz) = $rotquat->rotate_vector($ax,$ay,$az);
ok( 
	equal_fuzz($bx,$dx) && equal_fuzz($by,$dy) && equal_fuzz($bz,$dz),
	"Rotation maps start vector onto end vector"
);

my $squat = Math::Quaternion->new(0,1,2,3);
ok( Math::Quaternion::stringify($squat) eq "( 0 1 2 3 )",
	"Stringification works");


{
 my @axis = (0,0,1);
 my $rq1 = Math::Quaternion::rotation(pi/2,\@axis);   # 90  degrees about Z
 my $rq2 = Math::Quaternion::rotation(pi,\@axis);     # 180 degrees about Z

 my $interp = Math::Quaternion::slerp($rq1,$rq2,0.5); # 135 degrees about Z
 my ($ax,$ay,$az) = $interp->rotation_axis;
 ok( equal_fuzz(0,$ax) && equal_fuzz(0,$ay) && equal_fuzz(1,$az),
 	"Math::Quaternion::slerp() produces correct rotation axis");
 ok( equal_fuzz(0.75*pi,$interp->rotation_angle),
 	"Math::Quaternion::slerp() produces correct rotation angle");

 $rq2 = Math::Quaternion::rotation((pi/2)+1e-6,\@axis);
 $interp = Math::Quaternion::slerp($rq1,$rq2,0.75);
 ok( quatequal_fuzz($interp,
 	Math::Quaternion::plus(
		Math::Quaternion::scale($rq1,0.25),
		Math::Quaternion::scale($rq2,0.75)
	)),
	"Math::Quaternion::slerp works linearly for small angles");

  $rq1 = Math::Quaternion->new({ axis=>[0,0,1],angle=>0.25*pi});
  $rq2 = Math::Quaternion->new({ axis=>[0,0,1],angle=>1.5*pi});
  $interp = Math::Quaternion::slerp($rq1,$rq2,0.75);
  ($ax,$ay,$az) = $interp->rotation_axis;
 ok( equal_fuzz(0,$ax) && equal_fuzz(0,$ay) && equal_fuzz(-1,$az),
 	"Math::Quaternion::slerp() gives correct axis for -ve dp");
 ok( equal_fuzz(0.3125*pi,$interp->rotation_angle),
 	"Math::Quaternion::slerp() gives correct angle for -ve dp");


}