#!/usr/bin/perl -ws

our ($all,$h,$help,$no_filter,$RMS,$sort,$reorient);

if ($h or $help) {
  print "Simple USPEX/GULP to xyz converter

Usage: $0 gulp.log > gulp.xyz

Dependencies: perl

Options:

-all       All final coordinates of atoms will be printed,
           not only where cell parameters to be optimised

-no_filter Without this option adjacent similar structures will be filtered
-RMS       rmsd for filtering. Default -RMS=0.01      

-sort      structures will be sorted by energy. 
           It seems to be senseless -sort and -all together.

-reorient  Reorientation of the structures by inertia tenzor axes
\n";
  exit;
}

$RMS ||= 0.01;

my @mol;
my $count = 1;

my %massa = qw(
H  1.007947    He 4.0026022   Li 6.9412      Be 9.0121823   B  10.8117
C  12.01078    N  14.00672    O  15.99943    F  18.99840325 Ne 20.17976
Na 22.9897702  Mg 24.30506    Al 26.9815382  Si 28.08553    P  30.9737612
S  32.0655     Cl 35.4532     Ar 39.9481     K  39.09831    Ca 40.0784
Sc 44.9559108  Ti 47.8671     V  50.94151    Cr 51.99616    Mn 54.9380499
Fe 55.8452     Co 58.9332009  Ni 58.69342    Cu 63.5463     Zn 65.4094
Ga 69.7231     Ge 72.641      As 74.921602   Se 78.963      Br 79.9041
Kr 83.7982     Rb 85.46783    Sr 87.621      Y  88.905852   Zr 91.2242
Nb 92.906382   Mo 95.942      Ru 101.072     Rh 102.905502  Pd 106.421
Ag 107.86822   Cd 112.4118    In 114.8183    Sn 118.7107    Sb 121.7601
Te 127.603     I  126.904473  Xe 131.2936    Cs 132.905452  Ba 137.3277
La 138.90552   Ce 140.1161    Pr 140.907652  Nd 144.243     Sm 150.363
Eu 151.9641    Gd 157.253     Tb 158.925342  Dy 162.5001    Ho 164.930322
Er 167.2593    Tm 168.934212  Yb 173.043     Lu 174.9671    Hf 178.492
Ta 180.94791   W  183.841     Re 186.2071    Os 190.233     Ir 192.2173
Pt 195.0782    Au 196.966552  Hg 200.592     Tl 204.38332   Pb 207.21
Bi 208.980382  Th 232.03811   Pa 231.035882  U  238.028913
);

while (<>) {
  if (m/GENERAL UTILITY LATTICE PROGRAM/) {
    my (@frac,$a,$b,$c,$alpha,$beta,$gamma,$en,$cell_optimization);
    while (<>) {
      if (m/Cell parameters \(Angstroms\/Degrees\)/) {
        <>;
        $_=<>; ($a,$alpha) = (split)[2,5];
        $_=<>; ($b,$beta) = (split)[2,5];
        $_=<>; ($c,$gamma) = (split)[2,5];
      }
      if (m/Final enthalpy =\s+(\S+ eV)/) {
        $en = $1;
        #next if m/Job Finished/;
      }
      if (m/Final fractional coordinates of atoms/) {
        <> for 1..5;
        while (<>) {
          last if m/^---/;
          push @frac, [(split)[1,3,4,5] ];
        }
      }
      if (m/Final cell parameters and derivatives/) {
        $cell_optimization = 1;
        <>; <>;
        $_=<>; $a = (split)[1];
        $_=<>; $b = (split)[1];
        $_=<>; $c = (split)[1];
        $_=<>; $alpha = (split)[1];
        $_=<>; $beta  = (split)[1];
        $_=<>; $gamma = (split)[1];
      }
      if (m/Job Finished/) {
        last if ! $all && ! $cell_optimization;
        my $mol = frac2xyz(\@frac,$a,$b,$c,$alpha,$beta,$gamma);
        $mol->[0]{Energy} = $en;
        push @mol, $mol;
        print STDERR "\rExtracted structures: $count";
        $count++;
        last;
      }
    }
  }
}

print STDERR "\n";

unless ($no_filter) {
  my $count = 1;
  my @filter_mol = ($mol[0]);
  print STDERR "\rFiltered structures: $count";
  for (my $i=1; $i<@mol; $i++) {
    if (get_rms($filter_mol[-1], $mol[$i])>$RMS) {
      push @filter_mol, $mol[$i];
      print STDERR "\rFiltered structures: $count";
      $count++;
    }
  }
  print STDERR "\n";
  @mol = @filter_mol;
}

if ($sort) {
  print STDERR "Sorting by energy\n";
  @mol = 
  map {$_->[1]} 
  sort {$a->[0]<=>$b->[0]} 
  map {$_->[0]{Energy}=~m/(-?\d+(?:\.\d+)?)/;[$1,$_]} 
  @mol;
}

if ($reorient) {
  print STDERR "Reorientation by inertia tenzor\n";
  tenzor(@mol);
}

write_molden(@mol);

sub frac2xyz {
  my ($frac,$a,$b,$c,$alpha,$beta,$gamma)=@_;
  my ($sa,$sb,$sg) = map {sin($_/57.29578)} ($alpha,$beta,$gamma);
  my ($ca,$cb,$cg) = map {cos($_/57.29578)} ($alpha,$beta,$gamma);
  #print "$sa,$sb,$sg, $ca,$cb,$cg\n";
  my @xyz = ({});
  foreach (@$frac) {
    my ($at,$x,$y,$z) = @$_;
    my $X = $a*$x + $b*$cg*$y + $c*$cb*$z;
    my $Y = $b*$sg*$y + $c*($ca-$cb*$cg)/$sg*$z;
    my $Z = $c*sqrt(1-$ca**2-$cb**2-$cg**2+2*$ca*$cb*$cg)/$sg*$z;
    push @xyz, [$at,$X,$Y,$Z];
  }
  return \@xyz;  
}

sub write_molden {
  my $oldfh;
  
  if (@_ > 1 && !ref($_[-1])) {
    my $file = pop @_;
    open F, '>', $file or do {warn "Can't write to $file: $!\n"; return};
    $oldfh = select F;
  }
  
  foreach my $mol (@_) {
    my $N = $#{$mol};
    print " $N\n";
    print " Energy $mol->[0]{Energy}" if $mol->[0]{Energy};
    print "  Symmetry $mol->[0]{Symmetry}" if $mol->[0]{Symmetry};
    print "\n";
    for (my $i=1; $i<=$N; $i++) {
      printf " %-2s %12.8f %12.8f %12.8f\n", @{$mol->[$i]};
    }
  }
  
  if ($oldfh) {
    close F;
    select $oldfh;
  }
}

sub get_rms {
 ############################################################################
 ## Переделанная на перл coord.c Г.Сальникова
 ## Принимает две ссылки на геометрии (см. read_molden() из conformers)
 ## Возвращает RMSD между геометриями
 ## Во вторую ссылку записывает новую, сглаженную геометрию
 ## Использует глобальный хэш %massa с массами элементов (см. smooth)
 ############################################################################
  my ($mol0, $mol1) = @_;
  my ($i, $n, $rot);
  my (@m, @x0, @y0, @z0, @x1, @y1, @z1, $xc, $yc, $zc, $e, $e0,
    $mtot, $tg1, $tg2, $phi, $phix, $phiy, $phiz);
  my $M_PI = atan2(0,-1);
  my $FLT_EPSILON = 1e-9;
  my $DEBUG = 0;
  
  #/*** Input data preparation ***/
  $n = $#{$mol0};
  for ($i=0; $i<$n; $i++) {
    $m[$i] = $massa{$mol0->[$i+1][0]};
    die "Inconsistent data\n" if $massa{$mol1->[$i+1][0]} != $m[$i];
    $mtot += $m[$i];
    $x0[$i] = $mol0->[$i+1][1];
    $y0[$i] = $mol0->[$i+1][2];
    $z0[$i] = $mol0->[$i+1][3];
    $x1[$i] = $mol1->[$i+1][1];
    $y1[$i] = $mol1->[$i+1][2];
    $z1[$i] = $mol1->[$i+1][3];
  }
  
  #/*** 1-st translation to center of mass ***/

  $xc = $yc = $zc = 0;
  for ($i=0; $i<$n; $i++)
  {
    $xc += $m[$i]*$x0[$i];
    $yc += $m[$i]*$y0[$i];
    $zc += $m[$i]*$z0[$i];
  }
  $xc /= $mtot;
  $yc /= $mtot;
  $zc /= $mtot;

  for ($i=0; $i<$n; $i++)
  {
    $x0[$i] -= $xc;
    $y0[$i] -= $yc;
    $z0[$i] -= $zc;
  }

  if ($DEBUG) {
    printf ("1-st molecule in center of mass\n");
    for ($i=0; $i<$n; $i++) {
      printf "%2.0f   %10f   %10f   %10f\n", $m[$i], 
              $x0[$i], $y0[$i], $z0[$i];
    }
    printf "1-st center of mass translation on: (%10f, %10f, %10f)\n",
            -$xc, -$yc, -$zc;
  }

  #/*** 2-nd translation to center of mass ***/

  $xc = $yc = $zc = 0;
  for ($i=0; $i<$n; $i++)
  {
    $xc += $m[$i]*$x1[$i];
    $yc += $m[$i]*$y1[$i];
    $zc += $m[$i]*$z1[$i];
  }
  $xc /= $mtot;
  $yc /= $mtot;
  $zc /= $mtot;

  for ($i=0; $i<$n; $i++)
  {
    $x1[$i] -= $xc;
    $y1[$i] -= $yc;
    $z1[$i] -= $zc;
  }

  if ($DEBUG) {
    printf ("2-nd molecule in center of mass\n");
    for ($i=0; $i<$n; $i++) {
      printf "%2.0f   %10f   %10f   %10f\n", 
              $m[$i], $x1[$i], $y1[$i], $z1[$i];
    }
    printf "2-nd center of mass translation on: (%10f, %10f, %10f)\n",
            -$xc, -$yc, -$zc;
  }

  $e = 0;
  for ($i=0; $i<$n; $i++) {
    $e += $m[$i]*(($x1[$i]-$x0[$i])*($x1[$i]-$x0[$i])+
         ($y1[$i]-$y0[$i])*($y1[$i]-$y0[$i])+
         ($z1[$i]-$z0[$i])*($z1[$i]-$z0[$i]));
  }
  printf ("2*energy: %g\n", $e) if $DEBUG;

  for ($rot=1; ; $rot++)
  {
    $e0 = $e;

    #/*** Rotation around X ***/

    $tg1 = $tg2 = 0;
    for ($i=0; $i<$n; $i++)
    {
      $tg1 += $m[$i]*($y0[$i]*$z1[$i]-$z0[$i]*$y1[$i]);
      $tg2 += $m[$i]*($y0[$i]*$y1[$i]+$z0[$i]*$z1[$i]);
    }
    $phi = atan2 ($tg1, $tg2);

    for ($i=0; $i<$n; $i++)
    {
      $yc = $y1[$i]*cos($phi)+$z1[$i]*sin($phi);
      $zc = $z1[$i]*cos($phi)-$y1[$i]*sin($phi);
      $y1[$i] = $yc;
      $z1[$i] = $zc;
    }

    $e = 0;
    for ($i=0; $i<$n; $i++) {
      $e += $m[$i]*(($x1[$i]-$x0[$i])*($x1[$i]-$x0[$i])+
     ($y1[$i]-$y0[$i])*($y1[$i]-$y0[$i])+
     ($z1[$i]-$z0[$i])*($z1[$i]-$z0[$i]));
    }
    if ($DEBUG) {
      for ($i=0; $i<$n; $i++) {
        printf "%2.0f   %10f   %10f   %10f\n", 
                $m[$i], $x1[$i], $y1[$i], $z1[$i];
      }
      printf "after %d-th rotation around X on: %g (%g deg)\n2*energy: %g\n",
              $rot, $phi, $phi*180/$M_PI, $e;
    }

    $phix = $phi;

    #/*** Rotation around Y ***/

    $tg1 = $tg2 = 0;
    for ($i=0; $i<$n; $i++)
    {
      $tg1 += $m[$i]*($z0[$i]*$x1[$i]-$x0[$i]*$z1[$i]);
      $tg2 += $m[$i]*($z0[$i]*$z1[$i]+$x0[$i]*$x1[$i]);
    }
    $phi = atan2 ($tg1, $tg2);

    for ($i=0; $i<$n; $i++)
    {
      $xc = $x1[$i]*cos($phi)-$z1[$i]*sin($phi);
      $zc = $z1[$i]*cos($phi)+$x1[$i]*sin($phi);
      $x1[$i] = $xc;
      $z1[$i] = $zc;
    }

    $e = 0;
    for ($i=0; $i<$n; $i++) {
      $e += $m[$i]*(($x1[$i]-$x0[$i])*($x1[$i]-$x0[$i])+
     ($y1[$i]-$y0[$i])*($y1[$i]-$y0[$i])+
     ($z1[$i]-$z0[$i])*($z1[$i]-$z0[$i]));
    }
    if ($DEBUG) {
      for ($i=0; $i<$n; $i++) {
        printf "%2.0f   %10f   %10f   %10f\n", 
                $m[$i], $x1[$i], $y1[$i], $z1[$i];
      }
      printf "after %d-th rotation around Y on: %g (%g deg)\n2*energy: %g\n",
              $rot, $phi, $phi*180/$M_PI, $e;
    }

    $phiy = $phi;

    #/*** Rotation around Z ***/

    $tg1 = $tg2 = 0;
    for ($i=0; $i<$n; $i++)
    {
      $tg1 += $m[$i]*($x0[$i]*$y1[$i]-$y0[$i]*$x1[$i]);
      $tg2 += $m[$i]*($x0[$i]*$x1[$i]+$y0[$i]*$y1[$i]);
    }
    $phi = atan2 ($tg1, $tg2);

    for ($i=0; $i<$n; $i++)
    {
      $xc = $x1[$i]*cos($phi)+$y1[$i]*sin($phi);
      $yc = $y1[$i]*cos($phi)-$x1[$i]*sin($phi);
      $x1[$i] = $xc;
      $y1[$i] = $yc;
    }

    $e = 0;
    for ($i=0; $i<$n; $i++) {
      $e += $m[$i]*(($x1[$i]-$x0[$i])*($x1[$i]-$x0[$i])+
     ($y1[$i]-$y0[$i])*($y1[$i]-$y0[$i])+
     ($z1[$i]-$z0[$i])*($z1[$i]-$z0[$i]));
    }
    if ($DEBUG) {
      for ($i=0; $i<$n; $i++) {
        printf "%2.0f   %10f   %10f   %10f\n", 
               $m[$i], $x1[$i], $y1[$i], $z1[$i];
      }
      printf "after %d-th rotation around Z on: %g (%g deg)\n2*energy: %g\n",
        $rot, $phi, $phi*180/$M_PI, $e;
    }

    $phiz = $phi;

    last if (abs($phix) < $FLT_EPSILON &&
             abs($phiy) < $FLT_EPSILON &&
             abs($phiz) < $FLT_EPSILON &&
             ($e == $e0 || abs(($e-$e0)/($e+$e0)) < $FLT_EPSILON));
  }

  #/*** Output data preparation ***/

  for ($i=0; $i<$n; $i++) {
    $mol1->[$i+1][1] = $x1[$i];
    $mol1->[$i+1][2] = $y1[$i];
    $mol1->[$i+1][3] = $z1[$i];
  }
  if ($DEBUG) {
    for ($i=0; $i<$n; $i++) {
      printf "%2.0f   %10f   %10f   %10f\n", $m[$i], $x1[$i], $y1[$i], $z1[$i];
    }
    printf "2*energy: %g\n", $e;
  }

  #/*** Finished ***/
  
#  return sqrt($e);
  return $e;
}

sub tenzor {
 ###########################################################################
 ## Принимает массив ссылок (молекул) и модифицирует их по месту, 
 ## перемещая в центр масс и ориентируя по осям тензора инерции.
 ## Использует внешний хэш %massa и функцию jacobi().
 ###########################################################################
  my @eigen_num;
  
  #print STDERR "Orientation of molecula by inertia tenzor\n";
  
  foreach my $mol (@_) {
    my (@m,@atom,@x,@y,@z);
    my $N = $#{$mol};
    # Создаем массив атомных масс
    for (my $i=1; $i<=$N; $i++) {
      $atom[$i] = $mol->[$i][0];
      $x[$i] = $mol->[$i][1];
      $y[$i] = $mol->[$i][2];
      $z[$i] = $mol->[$i][3];
      $m[$i] = $massa{$atom[$i]};
    }

    # Вычисляем молекулярную массу
    my $M = sum(@m[1..$N]);
    #print "$M\n";

    # Перемещаем геометрию в центр масс
    my $sum;
    $sum = sum(map { $x[$_]*$m[$_] } 1..$N)/$M;
    $_ -= $sum for @x[1..$N];
    $sum = sum(map { $y[$_]*$m[$_] } 1..$N)/$M;
    $_ -= $sum for @y[1..$N];
    $sum = sum(map { $z[$_]*$m[$_] } 1..$N)/$M;
    $_ -= $sum for @z[1..$N];
    #&write_molden;

    #  Суть: молекулу представляем в виде эллипсоида инерции.
    #  Длина главных осей этого эллипсоида определяются собственными значениями 
    #  тензора инерции, а их направление - соответствующими собственными векторами. 
    #  Г. Голдстейн. Классическая механика. Глава 5.

    # Вычисляем тензор инерции
    # | Ixx Ixy Ixz |
    # | Ixy Iyy Iyz |
    # | Ixz Iyz Izz |
    my (@r2,$Ixx,$Ixy,$Ixz,$Iyy,$Iyz,$Izz);
    $r2[$_] = $x[$_]**2+$y[$_]**2+$z[$_]**2 for 1..$N;
    $Ixx = sum(map {$m[$_]*($r2[$_]-$x[$_]**2)} 1..$N);
    $Iyy = sum(map {$m[$_]*($r2[$_]-$y[$_]**2)} 1..$N);
    $Izz = sum(map {$m[$_]*($r2[$_]-$z[$_]**2)} 1..$N);
    $Ixy = -sum(map {$m[$_]*$x[$_]*$y[$_]} 1..$N);
    $Ixz = -sum(map {$m[$_]*$x[$_]*$z[$_]} 1..$N);
    $Iyz = -sum(map {$m[$_]*$y[$_]*$z[$_]} 1..$N);

    #($Ixx,$Ixy,$Ixz,$Iyy,$Iyz,$Izz) = (6,-2,2,5,0,7);

    #print "Тензор инерции:\n";
    #printf "%12.8f %12.8f %12.8f\n%12.8f %12.8f %12.8f\n%12.8f %12.8f %12.8f\n",
    #         $Ixx,  $Ixy,  $Ixz,   $Ixy,  $Iyy,  $Iyz,   $Ixz,  $Iyz,  $Izz;

    my @eigen = jacobi($Ixx,$Ixy,$Ixz,$Iyy,$Iyz,$Izz) or die "Eigen error\n";
    
    push @eigen_num, [sqrt($eigen[0][0]/$M),
                      sqrt($eigen[1][0]/$M),
                      sqrt($eigen[2][0]/$M)];
    # Преобразуем геометрию к новой системе координат (собственные вектора)
    my @new_geom;
    for (my $i=0; $i<$N; $i++) {
      for (0..2) {
        $new_geom[$_][$i] = $x[$i+1]*$eigen[$_][1][0] + 
                            $y[$i+1]*$eigen[$_][1][1] + 
                            $z[$i+1]*$eigen[$_][1][2];
      }
    }
    #write_molden();

    # Переставляем оси эллипсоида, чтобы хорошо смотрелось в молдене
    @x[1..$N] = @{$new_geom[1]};
    @y[1..$N] = @{$new_geom[2]};
    @z[1..$N] = @{$new_geom[0]};
  
    # Из новой геометрии делаем две молекулы (одну с отражением по z)
    my ($mol1,$mol2);
    for (my $i=1; $i<=$N; $i++) {
      $mol2->[$i][0] = $mol1->[$i][0] = $mol->[$i][0];
      $mol2->[$i][1] = $mol1->[$i][1] = $x[$i];
      $mol2->[$i][2] = $mol1->[$i][2] = $y[$i];
      $mol1->[$i][3] =  $z[$i];
      $mol2->[$i][3] = -$z[$i];
    }
    
    my $znakz = 1;
    #my $rms1 = get_rms($mol,$mol1);
    my $rms2 = get_rms($mol,$mol2);
    #print '=';
    #printf "%20.10f %20.10f\n", $rms1, $rms2;
    $znakz = -1 if $rms2 < 1e-8;
  
    # Перезаписываем молекулу
    for (my $i=1; $i<=$N; $i++) {
      $mol->[$i][1] = $x[$i];
      $mol->[$i][2] = $y[$i];
      $mol->[$i][3] = $znakz*$z[$i];
    }
  }
  #print "\n";
  return @eigen_num;
}
  
sub jacobi {
 ###########################################################################
 ## Собственные числа и векторы симметричной матрицы методом Якоби.
 ## Код из Жориной программы dte (C).
 ## Принимает массив ($Ixx,$Ixy,$Ixz,$Iyy,$Iyz,$Izz).
 ## Возвращает отсортированный (по убыванию собств. чисел) массив двухэлементных
 ## массивов. 1-й элемент - собств. число, 2-й - соответствующий ему вектор.
 ###########################################################################
  my @a = ([ $_[0],$_[1],$_[2] ],
           [ $_[1],$_[3],$_[4] ],
           [ $_[2],$_[4],$_[5] ]);
  my ($j, $iq, $ip, $i, $n);
  my ($tresh, $theta, $tau, $t, $sm, $s, $h, $g, $c);
  my $maxit = 50;

  $n = 3;

  my @v = ([1,0,0], [0,1,0], [0,0,1]);
  my @b = my @d = ($a[0][0],$a[1][1],$a[2][2]);
  my @z = (0,0,0);
  
  my $nrot = 0;
  for ($i=0; $i<$maxit; $i++) {
    
    $sm = abs($a[0][1]) + abs($a[0][2]) + abs($a[1][2]);
    
    if ($sm == 0) {
      return sort {$b->[0] <=> $a->[0]}
              ([$d[0], [$v[0][0],$v[1][0],$v[2][0]]],
              [$d[1], [$v[0][1],$v[1][1],$v[2][1]]],
              [$d[2], [$v[0][2],$v[1][2],$v[2][2]]]);
    }
    # The normal return, which relies on 
    # quadratic convergence to machine underflow. 
    
    $tresh = ($i < 4) ? 0.2*$sm/($n*$n) : 0;
    # ...on the first three sweeps. 
    # ...thereafter. 
    
    for ($ip=0; $ip<$n-1; $ip++) {
      for ($iq=$ip+1; $iq<$n; $iq++) {
        $g = 100*abs($a[$ip][$iq]);
        # After four sweeps, skip the rotation if the off-diagonal element is small. 
        if ($i>4 && abs($d[$ip])+$g == abs($d[$ip]) && abs($d[$iq])+$g == abs($d[$iq])) {
          $a[$ip][$iq] = 0;
        }
        elsif (abs ($a[$ip][$iq]) > $tresh) {
          $h = $d[$iq]-$d[$ip];
          if (abs($h)+$g == abs($h)) {
            $t = $a[$ip][$iq]/$h;
          }  # $t = 1/(2theta) 
          else
          {
            $theta = 0.5*$h/$a[$ip][$iq];
            $t = 1/(abs($theta)+sqrt(1+$theta*$theta));
            if ($theta < 0) {$t = -$t;}
          }
          $c = 1/sqrt(1+$t*$t);
          $s = $t*$c;
          $tau = $s/(1+$c);
          $h = $t*$a[$ip][$iq];
          $z[$ip] -= $h;
          $z[$iq] += $h;
          $d[$ip] -= $h;
          $d[$iq] += $h;
          $a[$ip][$iq] = 0;
          for ($j=0; $j<$ip; $j++) {  # Case of rotations 0 <= $j < p. 
            $g = $a[$j][$ip]; 
            $h = $a[$j][$iq];
            $a[$j][$ip] = $g-$s*($h+$g*$tau);
            $a[$j][$iq] = $h+$s*($g-$h*$tau);
          }
          for ($j=$ip+1; $j<$iq; $j++) {  # Case of rotations p < $j < q. 
            $g = $a[$ip][$j]; 
            $h = $a[$j][$iq];
            $a[$ip][$j] = $g-$s*($h+$g*$tau);
            $a[$j][$iq] = $h+$s*($g-$h*$tau);
          }
          for ($j=$iq+1; $j<$n; $j++) {  # Case of rotations q < $j < $n. 
            $g = $a[$ip][$j]; 
            $h = $a[$iq][$j];
            $a[$ip][$j] = $g-$s*($h+$g*$tau);
            $a[$iq][$j] = $h+$s*($g-$h*$tau);
          }
          for ($j=0; $j<$n; $j++) {
            $g = $v[$j][$ip]; 
            $h = $v[$j][$iq];
            $v[$j][$ip] = $g-$s*($h+$g*$tau);
            $v[$j][$iq] = $h+$s*($g-$h*$tau);
          }
          ++$nrot;
        }
      }
    }
    for ($ip=0; $ip<$n; $ip++) {
      $b[$ip] += $z[$ip];
      $d[$ip] = $b[$ip];      # Update $d with the sum of tapq, 
      $z[$ip] = 0;        # and reinitialize $z. 
    }
  }
  return undef;
}

sub sum {
  my $sum = 0;
  foreach (@_) {
    $sum += $_;
  }
  return $sum;
}