#!/usr/bin/perl -w

use strict;
use Math::Trig;
use Scalar::Util;
use clifford;

sub usage {

print <<\EoF;
Usage example:
  # compound rotation:  two 90 degree rotations
  # makes a 120 degree rotation about the 1,1,1 diagonal:
  echo -e "1 0 0 90° vrml 0 0 1 90° vrml mul @v" | rotmul
Result:
          0.57735 0.57735 0.57735 2.09440 = 120.0000°

Explanation:
  *) Push a rotation operator onto the stack, by
  giving four numbers in VRML format
     X Y Z theta
  followed by the "vrml" keyword.
  *) Push another rotation operator onto the stack,
  in the same way.
  *) Multiply them together using the "mul" keyword.
  *) Pop the result and print it in VRML format using
    the "@v" keyword

Note on VRML format:  X Y Z theta
  [X Y Z] is a vector specifying the axis of rotation,
  and theta specifies the amount of rotation around that axis.
  VRML requires [X Y Z] to be normalized as a unit vector,
  but we are more tolerant;  we will normalize it for you.
  VRML requires theta to be measured in radians, and we
  accept that, but we also accept
  theta measured in degrees if the value is
  suffixed by "°" (the degree symbol) or by "d"
  e.g. 0 0 1  90° is equivalent to 0 0 1  1.57080

  Input words can be spread across as many lines (or as few)
  as you wish.

Same example, with more output:
  echo -e "1 0 0 90° vrml dup @v dup @m 0 0 1 90° vrml mul dup @v @m" | rotmul
Result:
        1.00000 0.00000 0.00000 1.57080 =  90.0000°
           [  1.00000  0.00000  0.00000 ]
           [  0.00000  0.00000 -1.00000 ]
           [  0.00000  1.00000  0.00000 ]
        0.57735 0.57735 0.57735 2.09440 = 120.0000°
           [  0.00000  0.00000  1.00000 ]
           [  1.00000  0.00000  0.00000 ]
           [  0.00000  1.00000  0.00000 ]

Even fancier:  Multiply two vectors to create a bivector,
then use that to crank a vector:
  echo -e "1 0 0 vector 1 1 0 vector mul normalize 0 1 0 vector crank @" \
   | ./rotmul
Result:
    [-1, 0, 0]

Another example:  Calculate the angle between two vectors:
  echo -e "-1 0 0 vector 1 1 0 vector mul normalize rangle @a" | ./rotmul
Result:
    2.35619 = 135.0000°

Example:  Powers: Exponentiate a quaternion.  Find rotor that rotates only half as much:
   echo -e "1 0 0 vector 0 1 0 vector mul 2 mul dup rangle @a .5 pow dup rangle @a @" | ./rotmul
Result:
    1.57080 =  90.0000°
    0.78540 =  45.0000°
    1 + [0, 0, 1]§

Example:  Take the 4th root using pow, then take the fourth
power using direct multiplication of quaternions:
  echo "1 0 0 vector 0 1 0 vector mul dup @v
  .25 pow dup @v dup mul dup mul @v" | ./rotmul
Result
    0.00000 0.00000 1.00000 3.14159 = 180.0000°
    0.00000 0.00000 1.00000 0.78540 =  45.0000°
    0.00000 0.00000 1.00000 3.14159 = 180.0000°

More systematic testing:
   ./rotmul.test1

The following operators have been implemented:
EoF
  list_ops();
  exit(0);
}

######################
## Global variables ##
my $verbose = 0;
my @stack = ();
my $verb;                       # global, because used in error messages
my @ops = ();
my $deg = pi()/180.;            # one degree, measured in radians
my $hodge_sym = "\302\247";

sub print_vrml {
  my ($A) = @_;
  my ($w, $Ayz, $Azx, $Axy) = @$A;
## convert to the unenlightened normalization that VRML prefers:
  my $norm = sqrt($Ayz*$Ayz + $Azx*$Azx + $Axy*$Axy);
  if ($norm == 0) {
    $Ayz = 1; $norm = 1;
  }
  my $theta = 2.0 * acos($w);
  printf("    %7.5f %7.5f %7.5f %7.5f = %8.4f\302\260\n",
     $Ayz/$norm,
     $Azx/$norm,
     $Axy/$norm, $theta, $theta/$deg);
}

## Convert a rotor into a 3x3 rotation matrix

# Print a generic 3x3 matrix
sub print_matrix{
  my ($M) = @_;
  my ($A, $B, $C) = @$M;
  my @rotmat = (@$A, @$B, @$C);

## I hate it when perl formats numbers as "-0.00000".
## We are only printing 5 decimal places, so whacking things
## at the 1e-8 level should be quite harmless:
  for (my $ii = 0;  $ii < $#rotmat; $ii++){
    if (abs($rotmat[$ii]) < 1e-8) {
      $rotmat[$ii] = 0;
    }
  }
  my $f = "      [ %8.5f %8.5f %8.5f ]\n";
  printf("$f$f$f", @rotmat);
}

sub check_depth{
  my ($depth) = @_;
  my $nst = @stack;
  if ($depth > $nst) {
    die "$verb needs $depth things; got only $nst.\n";
  }
}

sub check_number{
  my ($depth) = @_;
  check_depth($depth);
  for (my $ii = 0; $ii < $depth; $ii++){
    my $type = ref($stack[$ii]);
    if ($type ne '') {
      die "$verb needs $depth numbers; got '$type' at depth $ii.\n";
    }
  }
  return 1;
}

sub check_thing{
  my ($depth, $goodtype) = @_;
  check_depth($depth);
  for (my $ii = 0; $ii < $depth; $ii++){
    my $type = myref($stack[$ii]);
    if ($type ne $goodtype) {
      die "$verb needs $depth things of type '$goodtype'; got '$type' at depth $ii.\n";
    }
  }
  return 1;
}

sub print_clif{
  my ($A) = @_;
  if (codot($A, $A) == 0) {
    print "    0\n";
    return;
  }
  my $didsome = 0;
# New clif packing rule
# 0, 1, 2, 3,  4, 5,  6,  7
# .  x, y, xy, z, xz, yz, xyz
  my ($As, $Ax, $Ay, $Axy, $Az, $Axz, $Ayz, $Aps) = @$A;
  if ($As ne 0) {
    print "    ";
    print $As;
    $didsome++;
  }

  my $vec = [$Ax, $Ay, $Az];
  if (codot($vec, $vec) ne 0) {
    print ($didsome? " + " : "    ");
    print "[$Ax, $Ay, $Az]";
    $didsome++;
  }

  my $biv = [$Ayz, -$Axz, $Axy];
  if (codot($biv, $biv) ne 0) {
    print ($didsome? " + " : "    ");
    my $Azx = - $Axz;
    print "[$Ayz, $Azx, $Axy]$hodge_sym";
    $didsome++;
  }

  if ($Aps ne 0) {
    print ($didsome? " + " : "    ");
    print "$Aps\302\247";
    $didsome++;
  }

  print "\n";
}

sub print_general{
  my ($thing, $showtype) = @_;
  my $type = myref($thing);
  if ($showtype) {
    print "$type: ";
  }
  if ($type eq 'ARRAY') {
    my @stuff = @$thing;
    print ("    (", join(', ', @stuff), ")\n");
  }
  else {
    print_clif(promote($thing));
  }
}

######################################################################
## populate the list of operators ............

sub list_ops{
  foreach my $cmd (@ops) {
    printf "%-7s %s\n", @$cmd[0], @$cmd[1];
  }
}

push @ops, ['listops', "list all operators", \&list_ops];

push @ops, ['pop', "remove top item from stack", sub {
  check_depth(1);
  shift @stack;
}];

push @ops, ['pi', "push pi onto the stack", sub{
  unshift @stack, pi();
}];

# Now a few unary operators:

push @ops, ['deg', "convert number from radians to degrees", sub {
  check_number(1);
  $stack[0] *= &pi/180;
}];

push @ops, ['rev', "clifford '~' operator, reverse basis vectors", sub {
  check_depth(1);
  $stack[0] = rev($stack[0]);
}];


push @ops, ['dup', "duplicate top item on stack", sub {
  check_depth(1);
  unshift @stack, $stack[0];
}];

push @ops, ['gorm', "gorm i.e. scalar part of V~ V", sub {
  check_depth(1);
  $stack[0] = codot($stack[0], $stack[0]);
}];

push @ops, ['norm', "norm i.e. sqrt(gorm}", sub {
  check_depth(1);
  $stack[0] = norm($stack[0]);
}];

push @ops, ['normalize', "divide top item by its norm", sub {
  check_depth(1);
  $stack[0] = normalize($stack[0]);
}];

push @ops, ['hodge',
  "hodge dual aka unary '$hodge_sym' (formerly '*') operator",
sub {
  check_depth(1);
  $stack[0] = hodge($stack[0]);
}];

push @ops, ['rangle', "calculate rotor angle", sub {
  check_thing(1, 'QUAT');
  $stack[0] = rangle($stack[0]);
}];

# Now some binary operators
push @ops, ['exch', "exchange top two items on stack", sub {
  check_depth(2);
  ($stack[0], $stack[1]) = ($stack[1], $stack[0]);
}];

push @ops, ['codot', "multiply corresponding components, then sum", sub {
  check_depth(2);
  my $B = shift @stack;
  $stack[0] = codot($stack[0], $B);
}];

push @ops, ['add', "add top two items on stack", sub {
  check_depth(2);
  my $B = shift @stack;
  $stack[0] = add($stack[0], $B);
}];

push @ops, ['mul',
"multiply top two items on stack (in subspace if possible)", sub {
  check_depth(2);
  my $B = shift @stack;
  my $A = $stack[0];
  my $Atype = myref($A);
  my $Btype = myref($B);
  if (0) {}
  elsif ($Atype eq 'plain_number') {
    $stack[0] = mul_by_scalar($B, $A);
  }
  elsif ($Btype eq 'plain_number') {
    $stack[0] = mul_by_scalar($A, $B);
  }
  elsif ($Atype eq 'VECTOR' && $Btype eq 'VECTOR') {
    $stack[0] = vecmul($A, $B);
  }
  elsif ($Atype eq 'QUAT' && $Btype eq 'QUAT') {
    $stack[0] = quatmul($A, $B);
  }
  else {
# If all else has failed, promote them both and
# invoke the general case:
    $stack[0] = clifmul(promote($A), promote($B), 0);
  }
}];

push @ops, ['cmul', "promote A and B to clifs, then multiply them", sub{
  check_depth(2);
  my $B = shift @stack;
  my $A = $stack[0];
  $stack[0] = clifmul(promote($A), promote($B), 0);
}];

push @ops, ['dot', "promote A and B to clifs, then take dot product", sub{
  check_depth(2);
  my $B = shift @stack;
  my $A = $stack[0];
  $stack[0] = clifmul(promote($A), promote($B), -1);
}];

push @ops, ['wedge', "promote A and B to clifs, then take wedge product", sub{
  check_depth(2);
  my $B = shift @stack;
  my $A = $stack[0];
  $stack[0] = clifmul(promote($A), promote($B), 1);
}];

push @ops, ['crank', "calculate R~ V R", sub {
  check_thing(1, 'VECTOR');
  my $V = shift @stack;
  check_thing(1, 'QUAT');
  $stack[0] = crank($stack[0], $V);
}];

push @ops, ['pow', "calculate Nth power of scalar or quat", sub {
  check_number(1);
  my $S = shift @stack;
  my $X = $stack[0];
  my $Stype = myref($S);
  my $Xtype = myref($X);
  if ($Xtype eq 'QUAT') {
    $stack[0] = quat_pow($X, $S);
  }
  elsif ($Xtype eq 'plain_number') {
    $stack[0] = $X ** $S;
  }
  else {
    die "Sorry, don't know how to take powers using '$Xtype' and '$Stype'\n";
  }
}];


# Now some constructors:

push @ops, ['vector', "construct a vector from three numbers", sub {
  check_number(3);
  my $Az = shift @stack;
  my $Ay = shift @stack;
  my $Ax = shift @stack;
  my $thing = [$Ax, $Ay, $Az];
  bless $thing, 'VECTOR';
  unshift @stack, $thing;
}];

push @ops, ['vrml', "construct a quaternion from VRML representation x,y,z,theta", sub {

## Let the VRML representation be X, Y, Z, theta,
## where [X,Y,Z] is a unit vector.
## Here (Aw; Ax, Ay, Az) is the multivector representation,
##  i.e. quaternion representation, *not* the VRML representation.
##  Specifically, w1=cos(theta/2) and x1 is X*sin(theta/2).
##  Note that the whole multivector (w1; x1, y1, z1)
##  (including the scalar part w1, not just the bivector part)
##  is normalized to unity.

  check_number(4);
  my $theta = shift @stack;
  my $Az = shift @stack;
  my $Ay = shift @stack;
  my $Ax = shift @stack;
# should already be normalized, but we force the issue:
  my $norm = sqrt($Ax*$Ax + $Ay*$Ay + $Az*$Az);
  if ($norm == 0) {
    $Ax = 1; $norm = 1; $theta = 0;
  }
  my $ns = sin($theta/2.0) / $norm;       # normalized sine
  my $rotor = [cos($theta/2.0), $Ax*$ns, $Ay*$ns, $Az*$ns];
  bless $rotor, 'QUAT';
  unshift @stack, $rotor;
}];

push @ops, ['clif', "construct a clif from 8 numbers in bitcode order",
sub {
  check_number(3);
  my $clif = [reverse(@stack[0 .. 7])];
  bless $clif, 'CLIF';
  unshift @stack, $clif;
}];

# Now some printout operators:

push @ops, ['dump', "show everything on stack, leave it unchanged", sub {
  my $nst = @stack;
  print "$nst items:\n";
  for (my $ii = 0; $ii < $nst; $ii++) {
    print_general($stack[$ii], 1);
  }
}];

push @ops, ['@', "show item of any type (then remove it)", sub {
  check_depth(1);
  print_general (shift @stack);
}];

push @ops, ['@m', "show quaternion, formatted as a rotation matrix (then remove it)", sub {
  check_thing(1, 'QUAT');
  print_matrix(rotmat(shift @stack));
}];

push @ops, ['@v', "show quaternion, formatted in VRML style (then remove it)", sub {
  check_thing(1, 'QUAT');
  print_vrml (shift @stack);
}];

push @ops, ['@a', "show angle, formatted in radian and degrees (then remove it)", sub {
  check_number(1);
  my $theta = shift @stack;
  printf("    %7.5f = %8.4f\302\260\n", $theta, $theta/$deg);
}];

sub tabulate_grade_of_product{
  print "grade of product:\n";
# Print column headers
  printf "%9s", '';
  for (my $jj = 0; $jj < $Ccomp; $jj++) {
    printf " %3d     ", $jj;
  }
  print "\n";
  printf "%9s", '(';
  for (my $jj = 0; $jj < $Ccomp; $jj++) {
    printf " %3d     ", bladegrade($jj);
  }
  print ")\n\n";

  for (my $ii = 0; $ii < $Ccomp; $ii++) {
    my $iigrade = bladegrade($ii);
    printf " %3d(%2d):", $ii, $iigrade;
    for (my $jj = 0; $jj < $Ccomp; $jj++) {
      my $jjgrade = bladegrade($jj);
      my $prod = $ii ^ $jj;
      my $flags = '';
      my $pgrade = bladegrade($prod);
      $flags .= ($pgrade == abs($iigrade - $jjgrade)) ? 'd' : '_';
      $flags .= ($pgrade == abs($iigrade + $jjgrade)) ? 'w' : '_';
      $flags .= (sign_of_product($ii, $jj) > 0) ? '+' : '-';
#     $flags .= (sign_of_product($jj, $ii) > 0) ? '+' : '-';
      printf " %3d %-4s", $pgrade, $flags;
    }
    print "\n";
  }
}

sub test_blademap{
  print "blademap:\n";
  for (my $ii = 0; $ii < $Ccomp; $ii++) {
    printf "%3d", $ii;
    my @map = blademap($ii);
    foreach my $jj (@map){
      printf " %3d", $jj;
    }
    print "\n";
  }
}

# used by sort_bitcodes
sub bitcode_compare{
  my $foo = bladegrade($a) <=> bladegrade($b);
  if ($foo) {
    return $foo;
  }
  return $a <=> $b;
}

0 || push @ops, ['sort_bitcodes', "sort bitcodes according to grade", sub{
  my @nums = (0 .. $Ccomp-1);
  print join(', ', @nums), "\n";
  print join(', ', sort {bitcode_compare} @nums), "\n";
}];

push @ops, ['play', "experiment with counters, general clif multiply", sub{
  print "Sign of product: ",
     sign_of_product(1, 2), ", ",
     sign_of_product(2, 1), "\n";
  test_blademap();
  print "...\n";
  tabulate_grade_of_product();
}];

######################################################################

main: {

  for my $arg (@ARGV){
    if ($arg eq '-h'){
      usage();
    } elsif ($arg eq '-v'){
      $verbose++;
    } else {
      print STDERR "Unrecognized option '$arg';  for help try rotmul -h\n";
      exit(1);
    }
  }
  my $s2 = sqrt(0.5);
## Start with unit scalar quaternion, i.e. zero rotation:
  @stack = ();

  liner: for (;;) {
    if (eof(STDIN)) {
      last liner;
    }
    my $line = <>;
    chomp $line;
    my @words = split(' ', $line);
    worder: while (@words) {
      my $degree = 0;
      my $pseudo = 0;
      my $word = shift @words;
# strip comments:
      if ($word =~ m'^#'){
        next liner;
      }
# Note $verb is a global variable.
      $verb = $word;
      if ($word eq 'VRML') {        # special case; VRML is often capitalized
        $verb = $word = 'vrml';
      }
      elsif ($word eq $hodge_sym || $word eq "\247"){
        $verb = $word = 'hodge';
      }

# strip out any suffixed hodge symbol (alt-single-quote)
      if ($word =~ s/$hodge_sym$//
          || $word =~ s/\247$//) {
        $pseudo++;
      }
# strip out degree symbol (alt-zero)
# do this BEFORE seeing if $word looks_like_number:
      if ($word =~ s/\357\277\275$//
       || $word =~ s/\302\260$//
       || $word =~ s/[d°]$// ){
         $degree++;
      }
      if (Scalar::Util::looks_like_number($word)){
        if ($degree) {
          $word *= &pi/180;
        }
        unshift @stack, $word;
        if ($pseudo) {
          $stack[0] = hodge($stack[0]); # convert scalar to pseudoscalar
        }
        next worder;
      }
# Here if the word happens to be a verb.

      foreach my $cmd (@ops) {
        if ($verb eq @$cmd[0]){
          &{@$cmd[2]};
          next worder;
        }
      }
      die "Unrecognized verb '$verb'\n";
    }  ## end loop over words
  }    ## end loop over lines
  exit(0);
}