#!/opt/perl/bin/perl
use strict;
use GOA::Stacov;
use GPS::DATES;
use Matrix;
use File::Basename;
use Pod::Text::Termcap;
use Getopt::Long;
my $progname = basename $0;
my ($stacov_in,$stacov_out,$fpole,$help);
my ($R,$W,$wx,$wy,$wz,$Tr);
my $args = @ARGV;
GetOptions( 'i:s' => \$stacov_in,
            'o:s' => \$stacov_out,
            'p:s' => \$fpole,
            'wx:f' => \$wx,
            'wy:f' => \$wy,
            'wz:f' => \$wz,
            'help' => \$help,
          ) or die "Error in options\n";

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

if ( $fpole ) {
  ($R,$wx,$wy,$wz,$Tr) = wxyz($fpole);
}

if ( ! $wx ||  ! $wy || ! $wz ) {
   usage();
   exit 1;
}
 
$W = new Matrix ([ 0   ,  $wz , -$wy  ],
                 [-$wz ,  0   ,  $wx  ],
                 [ $wy , -$wx ,  0    ] )*$R;
                      
my $Wt = $W->transpose;

my $I = new Matrix()->eye(3);
 

my $s_in  = GOA::Stacov->new( file => $stacov_in );
   $s_in->remove( pname => [qw(aas aac ass asc)] );
   $s_in->print( file => "$stacov_in.tmp");

my $t0 =get_date($s_in->date,'dy');

my $s_out = GOA::Stacov->new( file => "$stacov_in.tmp" );

my $ipar=1;
my $npar = $s_in->npar;
my (@xi,@vi,$i,$j,$ii,$jj,$rho,$sig_i,$sig_j,$sta,$pname,$cname,$yy,$y,$doy,$dt,$t,$Phi);
my ($xyz_in,$xyz_in_sig,$xyz,$xyz_sig,$vxyz,$vxyz_sig,$vcov,$xcov,$xvcov,$cc);
my $m2mm=1e3;
while ( $ipar <=  $npar ) {
    @xi = ($ipar .. $ipar+2);
    $sta = $s_in->site($ipar);
    $pname = $s_in->pname($ipar); 
    $cname = $s_in->cname($ipar);
    if ( $pname eq 'STA' ) {
      $xyz_in     = new Matrix ([ map { $m2mm*$s_in->val($_) } @xi ])->transpose;
      $xyz_in_sig = new Matrix ([ map { $m2mm*$s_in->sig($_) } @xi ])->transpose;
      $vxyz = $s_in->xyz_vel($sta)->transpose;
	  $vxyz = $vxyz + $W*$xyz_in ; #- $Tr;
      $xcov  = $s_in->cov($sta)*($m2mm*$m2mm);
      $vcov  = $s_in->cov($sta,$sta,'VEL');
	  $vcov = $vcov + $W*$xcov*$Wt;
      $vxyz_sig = [ map { sqrt($vcov->[$_][$_]) } ( 0..2 ) ];
      @vi = map { $_->{ipar} } grep { $_->{pname} eq 'VEL' } $s_in->par($sta);
      $s_out->par( site => $sta,
                   pname => 'VEL',
                   xyz  => $vxyz->transpose,
                   xyz_sig => $vxyz_sig,
                 ); 
    }
    
    $ipar+=3;
}

if ( $stacov_out ) {
   $s_out->print( file => $stacov_out); 
}
else {
  $s_out->print;
}

unlink ("$stacov_in.tmp");

sub wxyz {
  my $file = shift;
  my $w=new Matrix();
  my $tr = new Matrix()->zeros(3);
  open(my $fh,'<',$file) or die "Couldn't open $file $!\n";
  
  while( <$fh> ) {
     chomp;
     s/^\s+//;
     s/\s+$//;
     s/#.*$//;
     next unless length;
     if ( /Wx\s+=(.*)/i ) {
       $w->[0]=$1; 
     }
     if ( /Wy\s+=(.*)/i ) {
       $w->[1]=$1; 
     }
     if ( /Wz\s+=(.*)/i ) {
       $w->[2]=$1; 
     }
     if ( /Tr_x\s+=(.*)/i ) {
       $tr->[0]=$1; 
     }
     if ( /Tr_y\s+=(.*)/i ) {
       $tr->[1]=$1; 
     }
     if ( /Tr_z\s+=(.*)/i ) {
       $tr->[2]=$1; 
     }
  }
  my $Pi = 4*atan2(1,1); 
  my $d2r = $Pi/180.0;
  if ( @$w ) {
    #my $R = sqrt( $$w[0]*$$w[0]+$$w[1]*$$w[1]+$$w[2]*$$w[2] );
    my $R = sqrt( $w.$w );
    #@$w = map { $_/$R } @$w;
	$w = $w/$R;
    return ($R*$d2r*1e-6, @$w,$tr->transpose)
  }
  else {
    die "No pole components found in $file\n";
  }
}

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

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

$progname 

=head1 OPTIONS

=over 3

=item -i input_stacov

=item -o output_stacov 

=item -p stacov.pole  

=back

=head1 EXAMPLES

=over 3

=item $progname -i itrf05.stacov -o na.stacov -p itrf05.pole  


=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
}

