#!/usr/bin/env perl

use Math::Trig;
use strict;
use warnings;

use constant DEG_IN_RAD => 57.2957795130823;
 
my $alpha = 76 / DEG_IN_RAD;	# RA of input
my $delta = 12 / DEG_IN_RAD;	# Dec of input

my ($alphaP, $deltaP, $phiP);

# Generate quaternion
my @p;
$p[0] = cos($alpha) * cos($delta);# x
$p[1] = sin($alpha) * cos($delta);# y
$p[2] = sin($delta);		# z
$p[3] = 0;			# Rotation = 0

# l2,b2
#$alphaP = 180 - 192.85948;
#$deltaP = 90 - 27.12825;
#$phiP = 90 + 32.93192;

# Ecliptic
#my $T = (2000.0 - 1900)/100;   # Centuries from 1900
#$alphaP = 270;
#$deltaP = ((23+27/60+8.26/3600) - 46.845/3600*$T - 0.0059/3600*$T*$T + 0.00181/3600*$T*$T*$T);
##$deltaP = 23.4392911111;
#$phiP = 90;

# Precession
my $T = 2;
$alphaP = 180 + (0.6406161*$T + 0.0000839*$T*$T + 0.0000050*$T*$T*$T);
$deltaP = (0.5567530*$T - 0.0001185*$T*$T - 0.0000116*$T*$T*$T);
$phiP = 180 + (0.6406161*$T + 0.0003041*$T*$T + 0.0000051*$T*$T*$T);


# Correct Euler angles to rotation angles
#$alphaP = (180 - $alphaP) / DEG_IN_RAD;
#$deltaP = (90 - $deltaP) / DEG_IN_RAD;
#$phiP = (90 + $phiP) / DEG_IN_RAD;

$alphaP /= DEG_IN_RAD;
$deltaP /= DEG_IN_RAD;
$phiP /= DEG_IN_RAD;

# Calculate quaternion for the rotation
my @r;
my ($phi, $theta);

# Rotate about the z axis by alpha_p
$r[0] = 0 * sin($alphaP/2);	# x
$r[1] = 0 * sin($alphaP/2);	# y
$r[2] = 1 * sin($alphaP/2);	# z
$r[3] = cos($alphaP/2);		# Rotation
@p = rotate(\@p, \@r);

$phi = atan2($p[1], $p[0]) * DEG_IN_RAD; # Longitude
$theta = asin($p[2]) * DEG_IN_RAD; # Latitude
print "$phi $theta\n";

# Rotate about the y axis by delta_p
$r[0] = 0 * sin($deltaP/2);	# x
$r[1] = 1 * sin($deltaP/2);	# y
$r[2] = 0 * sin($deltaP/2);	# z
$r[3] = cos($deltaP/2);		# Rotation
@p = rotate(\@p, \@r);

$phi = atan2($p[1], $p[0]) * DEG_IN_RAD; # Longitude
$theta = asin($p[2]) * DEG_IN_RAD; # Latitude
print "$phi $theta\n";

# Rotate about the z axis by phi_p
$r[0] = 0 * sin($phiP/2);	# x
$r[1] = 0 * sin($phiP/2);	# y
$r[2] = 1 * sin($phiP/2);	# z
$r[3] = cos($phiP/2);		# Rotation
@p = rotate(\@p, \@r);

$phi = atan2($p[1], $p[0]) * DEG_IN_RAD; # Longitude
$theta = asin($p[2]) * DEG_IN_RAD; # Latitude
print "$phi $theta\n";


# Perform a rotation using quaternions
sub rotate
{
    my ($p_ref, $r_ref) = @_;
    my @p = @$p_ref;		# Point to rotate (quaternion)
    my @r = @$r_ref;		# Rotation (quaternion)

    my @rConj = (- $r[0], - $r[1], - $r[2], $r[3]); # Quaternion conjugate

    my @rp = quaternionMultiply(\@r, \@p);
    my @rprConj = quaternionMultiply(\@rp, \@rConj);

    return @rprConj;
}

# Multiply two quaternions: pq
sub quaternionMultiply
{
    my ($p_ref, $q_ref) = @_;
    my @p = @$p_ref;
    my @q = @$q_ref;
    my @r;

    print "p: $p[0] $p[1] $p[2] $p[3]\n";
    print "q: $q[0] $q[1] $q[2] $q[3]\n";

    $r[0] =   $q[3]*$p[0] + $q[2]*$p[1] - $q[1]*$p[2] + $q[0]*$p[3];
    $r[1] = - $q[2]*$p[0] + $q[3]*$p[1] + $q[0]*$p[2] + $q[1]*$p[3];
    $r[2] =   $q[1]*$p[0] - $q[0]*$p[1] + $q[3]*$p[2] + $q[2]*$p[3];
    $r[3] = - $q[0]*$p[0] - $q[1]*$p[1] - $q[2]*$p[2] + $q[3]*$p[3];

    print "r: $r[0] $r[1] $r[2] $r[3]\n";

    return @r;
}

__END__