#!/opt/perl/bin/perl
use strict;
use Data::Dumper;
use Smart::Comments;
use GOA::Stacov;
use Matrix;
use File::Basename;
use Pod::Text::Termcap;
use Getopt::Long;
my $progname = basename $0;
my ($stacov_file,$fout,$fapr,$help);
my $antipodes=0;
my $apr={};
my $args = @ARGV;
GetOptions( 'i:s' => \$stacov_file,
            'o:s' => \$fout,
            'a'   => \$antipodes,
            'apr:s' => \$fapr,
            'help' => \$help,
          ) or die "Error in options\n";

if ( !$args || $help ) {
  usage();
  exit 1;
}

if ( ! $stacov_file ) {
 die "No stacov file supplied\n";
 usage(); exit 1;
}

my $stacov = GOA::Stacov->new( file => $stacov_file );

my @sites = $stacov->sites;

if ( -e $fapr ) {
  $apr =  read_apr($fapr);
}


#print "@sites\n";

my $Ai    = new Matrix()->zeros(3,3);
my $AtWA  = new Matrix()->zeros(3,3); 
my $AtWV  = new Matrix()->zeros(3,1);
my ($AtW,$A,$xyz_sig,$xyz,$vxyz,$vxyz_sig,$W,$R);
my $Pi = 4*atan2(1,1);
my $R  = 6378136.3 *1e3;               # m IERS Tech Note 13 pg 5
foreach my $s ( @sites ) { ### building N. Eq. [===|  ] % done
   $xyz = $stacov->xyz($s)*1e3;
   $vxyz = $stacov->xyz($s,'VEL')->transpose;
   $W    = $stacov->cov($s,$s,'VEL')->inv;
   $A = new Matrix( [ 0            , $xyz->[2]  , -$xyz->[1] ],
                    [ -$xyz->[2]   , 0          , $xyz->[0]  ],
					[ $xyz->[1]    , -$xyz->[0] , 0          ]);
   
   $AtW = $A->transpose->multiply($W);
   
   $A = $AtW->multiply($A);
   
   $AtWA = $AtWA->add($A);
   $AtWV = $AtWV->add($AtW->multiply($vxyz));
}

foreach my $s ( keys %$apr ) { ### building N. Eq. [===|  ] % done
   print "working on $s\n";
   $xyz = $apr->{$s}{xyz}*1e3;
   $vxyz = $apr->{$s}{vxyz}->transpose; #->multiply(-1);
   $W    = new Matrix()->eye(3,3);
   $A = new Matrix( [ 0            , $xyz->[2]  , -$xyz->[1] ],
                    [ -$xyz->[2]   , 0          , $xyz->[0]  ],
					[ $xyz->[1]    , -$xyz->[0] , 0          ]);
   
   $AtW = $A->transpose->multiply($W);
   
   $A = $AtW->multiply($A);
   
   $AtWA = $AtWA->add($A);
   $AtWV = $AtWV->add($AtW->multiply($vxyz));
}


   $antipodes= $antipodes ? -1 : 1;
my $deg = 180/$Pi;
my $AtWA_inv = $AtWA->inv;
my $Omega = $AtWA_inv*($AtWV)*($deg*1e6);
   $Omega = $Omega->transpose*$antipodes;
my $plate = 'noam';
my ($Wx,$Wy,$Wz) = @$Omega;
my $wxy=$Wx**2+$Wy**2;
my $lon = atan2($Wy,$Wx)*$deg;
my $lat = atan2($Wz,sqrt($wxy))*$deg;
my $w   = sqrt($wxy+$Wz**2)*$antipodes;
my $lon2= $lon < 0 ? $lon+360: $lon;
my $pv_header ="               lat(deg) lon(deg) magn(deg/Myr)\n";
my $pv = $pv_header;
$pv.= sprintf"$plate absolute % 7.2f % 7.2f % 6.4f\n",$lat,$lon2,$w;

my $Wout = qq(
File: $stacov_file
Sites: @sites
Wx = $Wx
Wy = $Wy
Wz = $Wz 
(lat,lon,elon,w) = ($lat,$lon, $lon2, $w)
);
my $fpole = $stacov_file;
$fpole =~ s/\.stacov//;
print qq(
________________
$fpole.pole
________________
$Wout
________________
);
print qq(
________________
$fpole.pv
________________
$pv
________________
);

open(my $gh,'>',"$fpole.pole") or die "Couldn't open $fpole.pole $!\n";
print $gh $Wout;
open(my $hh,'>',"$fpole.pv") or die "Couldn't open $fpole.pv $!\n";
print $hh $pv;

sub read_apr {
  my $file = shift;
  open(my $fh, '<', $file ) or die "couldn't open $file $!\n";
  my %apr = ();
  my %sta = ();
  @sta{@sites}=@sites;
  while( <$fh> ) {
     chomp;
     s/(#|\*).*$//;
     s/^\s+//;
     s/\s+$//;
     next unless length;
     my ($site,$x,$y,$z,$vx,$vy,$vz,$epoch) = split(' ');
     $site = unpack('a4',$site);
     next if ! exists $sta{$site};
     $apr{$site}{xyz}=new Matrix([$x,$y,$z]);
     $apr{$site}{vxyz}=new Matrix([$vx,$vy,$vz]);
     $apr{$site}{epoch}=$epoch;
  }
  return \%apr
}

sub norm {
  my $v = shift;
  my $norm = 0;
  
  for ( 0..2 ) { $norm += $v->[$_]*$v->[$_]; }
  
  return sqrt($norm)
  
}

sub usage {
  my $pod=Pod::Text::Termcap->new();
  $pod->parse_from_filehandle(man_page());
}

sub man_page {
my $man =qq(
=head1 NAME

$progname

script_template

=head1 OPTIONS

=over 3

=item -i input_file

=item -o output_file

=back

=head1 EXAMPLES

=over 3

=item $progname options 


=back

=head1 AUTHOR

Victor Marcelo Santillan <marcelo\@geology.cwu.edu>

=head1 COPYRIGHT

Copyright \(c\) 2007 

Central Washington University

=cut
);
open(my $fh,"<",\$man ) or die "$!\n";
return $fh
}

