#! /usr/bin/perl -w
use strict;
use warnings;
use Getopt::Long;           # options
use POSIX;
#---/----1----/----2----/----3----/----4----/----5----/----6----/----7----/----8----/----9----/----0
# ... notes
#---/----1----/----2----/----3----/----4----/----5----/----6----/----7----/----8----/----9----/----0
#:1101.11,  JG, begin development
#:1102.01,  JG, added logic in subroutines print11 and print21 as an attempt to make the script
#:          compatible with Windows. However, the variable $os may take on different definitions
#:          depending on the version of the Windows OS. Two cases that are known to me, that is
#:          $os = MSWin32 and $os =~ /win/i are included here. Other cases may need to be added
#:          as they are discovered.
#---/----1----/----2----/----3----/----4----/----5----/----6----/----7----/----8----/----9----/----0

if ( @ARGV != 2 ) {
  print "\n";
  print "Usage: perl apply_shifts.pl <shifts> <sinex>\n";
  print "\n";
  print "This script reads a table of ENU position corrections (shifts) and applies them to the\n";
  print "solution (sinex). The shifts are, of course, rotated to XYZ.\n";
  print "\n";
  print "The format of the shifts table is:\n";
  print "\n";
  print " ALGO  A    3  281.929  45.956  AOAD/M_B        NONE    0.00   0.00   0.00\n";
  print " ALGO  A    4  281.929  45.956  AOAD/M_T        NONE    0.00   0.00   0.00\n";
  print " ALGO  A    5  281.929  45.956  AOAD/M_T        NONE    0.00   0.00   0.00\n";
  print " ALGO  A    6  281.929  45.956  AOAD/M_T        NONE    0.00   0.00   0.00\n";
  print " ALGO  A    7  281.929  45.956  AOAD/M_T        NONE    0.00   0.00   0.00\n";
  print "\n";
  print "where,\n";
  print "\n";
  print "field 1:  4-char id\n";
  print "field 2:  pt id\n";
  print "field 3:  solution number\n";
  print "field 4:  longitude (decimal degrees)\n";
  print "field 5:  latitude (decimal degrees)\n";
  print "field 6:  antenna name\n";
  print "field 7:  radome name\n";
  print "field 8:  E correction (in mm)\n";
  print "field 9:  N correction (in mm)\n";
  print "field 10: U correction (in mm)\n";
  print "\n";
  print "\n";
  print "Example:\n";
  print "\n";
  print "perl apply_shifts.pl shifts.dat igs05-atx.SNX > igs08-atx.SNX 2> log\n";
  print "\n";
  print "The new sinex is writted to standard output. In the example above, the standard\n";
  print "output is captured by igs08.atx-calibrated SNX. The log file contains diagnostic\n";
  print "information about errors encountered during reading of the shifts.dat and the\n";
  print "input sinex.\n";
  print "\n";
  print "\n";
  exit;
}
  

# ... set variables
#     -------------
my $corr = $ARGV[0];
my $snx = $ARGV[1];
my $ant = ();
my $code = ();
my $code_pt = ();
my $code_pt_soln = ();
my $coeff = ();
my $d2r = ();
my $domes = ();
my $dome = ();
my $dE = ();
my $dN = ();
my $dU = ();
my $dX = ();
my $dY = ();
my $dZ = ();
my $e_corr = ();
my $flg = ();
my $i = ();
my $id = ();
my $lam_d = ();
my $lam_r = ();
my $lat = ();
my $lon = ();
my $n_corr = ();
my $phi_d = ();
my $phi_r = ();
my $pt = ();
my $sig = ();
my $soln = ();
my $S = ();
my $str = ();
my $str_val = ();
my $str_sig = ();
my $sv1 = ();
my $sv2 = ();
my $tref = ();
my $type = ();
my $unit = ();
my $u_corr = ();
my $val = ();
my $v1 = ();
my $v2 = ();
my $x_corr = ();
my $y_corr = ();
my $z_corr = ();
my %H_lon = ();
my %H_lat = ();
my %H_x_corr = ();
my %H_y_corr = ();
my %H_z_corr = ();


# ... set constants
#     -------------
my $pi = 3.141592653589793;
$d2r = $pi/180;
my $os = $^O;
print STDERR "$os\n";


# ... read/store position corrections
#     -------------------------------
open (CORR, "<$corr") ||
      die "\n***** ERROR: via open (r) of file $corr !\n             $!\n\n";
while (<CORR>) {
  next if ( m/^#CODE/ );
  print STDERR "$_";

  # ... read record
  ($code,$pt,$soln,$lon,$lat,$ant,$dome,$e_corr,$n_corr,$u_corr)=split ' ', $_;
  $code_pt_soln=sprintf("%4s%1s%d",$code,$pt,$soln);

  # ... rotate correction from ENU to XYZ
  $lam_d = $lon;
  $lam_r = $lam_d*$d2r;
  $phi_d = $lat;
  $phi_r = $phi_d*$d2r;
  ($dX,$dY,$dZ)=&enu2xyz($lam_r,$phi_r,$e_corr,$n_corr,$u_corr);

  print STDERR "$lam_d $phi_d ENU: $e_corr $n_corr $u_corr\n";
  print STDERR "$lam_d $phi_d XYZ: $dX $dY $dZ\n\n";

  # ... store correction in XYZ
  $H_x_corr{$code_pt_soln} = $dX;
  $H_y_corr{$code_pt_soln} = $dY;
  $H_z_corr{$code_pt_soln} = $dZ;
}
close CORR;

# ... read SNX and apply position shifts to SOLUTION/ESTIMATE block
#     -------------------------------------------------------------
$i=0; $flg=0;
open (SNX, "<$snx") ||
      die "\n***** ERROR: via open (r) of file $snx !\n             $!\n\n";
while (<SNX>) {
  if ( (substr $_, 0, 18) eq '+SOLUTION/ESTIMATE' ) {

    # ... print current record plus one, then read into buffer the next record
    print $_;
    $_ = <SNX>;
    print $_;
    $_ = <SNX>;

    # set flag=1 to start applying corrections
    $flg=1;
  }

  if ( (substr $_, 0, 18) eq '-SOLUTION/ESTIMATE' ) {

    # ... set flag=0 to stop applying corrections
    $flg=0;
  }

  if ( $flg == 0 ) { 

    # ... print current record
    print $_;

  } else {  # apply corrections

    # ...  count of record number
    $i = $i + 1;
  
    # ... split buffer
    ($id,$type,$code,$pt,$soln,$tref,$unit,$S,$val,$sig)=split ' ', $_;
    $code_pt_soln=sprintf("%4s%1s%d",$code,$pt,$soln);

    if (  (substr $_,7,3) eq 'STA' ) {
      print STDERR "$_";

      # ... apply correction, if any
      if ( defined $H_x_corr{$code_pt_soln} ) {
        if ( $type eq 'STAX' ) {
          $val = $val + $H_x_corr{$code_pt_soln}/1000;
        }
        if ( $type eq 'STAY' ) {
          $val = $val + $H_y_corr{$code_pt_soln}/1000;
        }
        if ( $type eq 'STAZ' ) {
          $val = $val + $H_z_corr{$code_pt_soln}/1000;
        }
      }
    }
    # ... print new record
    $str_val=print21($val);
    $str_sig=print11($sig);
    $str=sprintf("%6d %-6s %4s %2s %4d %s %-4s %d %s %s\n",$i,$type,$code,$pt,$soln,$tref,$unit,$S,$str_val,$str_sig);
    print "$str";
  }
}
close SNX;
exit;


####################################################################################################
####################################################################################################
#######################################     subroutines     ########################################
####################################################################################################
####################################################################################################

#-------------------------------------------------------------------------------
# Subroutine : print11
#
# Purpose    : Print a float number in this format : 0.24036E-03
# Author     : J. Griffiths
# Created    : 12-Jan-2011
# Changes    : 01-Feb-2001; added P. Rebischung's functionality for use on
#            : Windows
#
# In         : $x : Float number
# Out        : $s2 : Output string
# SR called  : 
#-------------------------------------------------------------------------------
sub print11 {
  my $x = shift;
  my $s = sprintf("%.5E", $x);
  my $s2;

  if ($os =~ 'MSWin32' || $os =~ '/win/i') {  # Windows
    $s = sprintf("%.5e", $x);
    $s2 = substr($s, 0, 9).substr($s, 10, 2);
  } else {               # UNIX/Linux 
    if ( $x >= 1 ) {
      $s2 .= '0.'.substr($s, 0, 1).substr($s, 2, 4).'E'.(sprintf"%+03d",(substr($s, 8, 3)+1));
    } elsif ( $x < 1 ) {
      $s2 .= '0.'.substr($s, 0, 1).substr($s, 2, 4).'E'.(sprintf"%+03d",(substr($s, 8, 3)+1));
    }
  }
  #print "s1: $s\n";
  #print "s2: $s2\n";
  return $s2;
}

#-------------------------------------------------------------------------------
# Subroutine : print21
#
# Purpose    : Print a float number in this format : 0.409721665526662E+07
# Author     : J. Griffiths
# Created    : 11-Jan-2011
# Changes    : 01-Feb-2001; added P. Rebischung's functionality for use on
#            : Windows
#
# In         : $x : Float number
# Out        : $s2 : Output string
# SR called  : 
#-------------------------------------------------------------------------------
sub print21 {
  my $x = shift;
  my $s = sprintf("%.14E", $x);
  my $s2;
  if ($os =~ 'MSWin32' || $os =~ '/win/i') {  # Windows
    $s = sprintf("%.14e", $x);
    $s2;
    if ($x >= 0) {
	  $s2 .= ' '.substr($s, 0, 18).substr($s, 19, 2);
    } else {
	  $s2 .= substr($s, 0, 19).substr($s, 20, 2);
    }
  } else {               # UNIX/Linux
    if ($x >= 0 && $x > 1 ) {
      $s2 .= '0.'.substr($s, 0, 1).substr($s, 2, 14).'E'.(sprintf"%+03d",(substr($s, 17, 3)+1));
    } elsif ($x >= 0 && $x < 1 ) {
      $s2 .= '0.'.substr($s, 0, 1).substr($s, 2, 14).'E'.(sprintf"%+03d",(substr($s, 17, 3)+1));
    } elsif ($x < 0 && $x < -1 ) {
      $s2 .= '-.'.substr($s, 1, 1).substr($s, 3, 14).'E'.(sprintf"%+03d",(substr($s, 18, 3)+1));
    } elsif ($x < 0 && $x > -1 ) {
      $s2 .= '-.'.substr($s, 1, 1).substr($s, 3, 14).'E'.(sprintf"%+03d",(substr($s, 18, 3)+1));
    }
  }
  #print "s1: $s\n";
  #print "s2: $s2\n";
  return $s2;
}

#-------------------------------------------------------------------------------
# Subroutine : xyz2enu
#
# Purpose    : Rotate differential from XYZ to ENU
# Author     : J. Griffiths
# Created    : 11-Jan-2011
# Changes    :
#
# In         : $lam_r, $phi_r, $dX, $dY, $dZ
# Out        : $dE, $dN, $dU
# SR called  : 
#-------------------------------------------------------------------------------
sub xyz2enu {
  ($lam_r,$phi_r,$dX,$dY,$dZ) = @_;
  $dE = -sin($lam_r)*$dX + cos($lam_r)*$dY;
  $dN = -sin($phi_r)*cos($lam_r)*$dX - sin($phi_r)*sin($lam_r)*$dY + cos($phi_r)*$dZ;
  $dU =  cos($phi_r)*cos($lam_r)*$dX + cos($phi_r)*sin($lam_r)*$dY + sin($phi_r)*$dZ;
  return($dE,$dN,$dU);
}

#-------------------------------------------------------------------------------
# Subroutine : enu2xyz
#
# Purpose    : Rotate differential from ENU to XYZ
# Author     : J. Griffiths
# Created    : 11-Jan-2011
# Changes    :
#
# In         : $lam_r, $phi_r, $dE, $dN, $dU
# Out        : $dX, $dY, $dZ
# SR called  : 
#-------------------------------------------------------------------------------
sub enu2xyz {
  ($lam_r,$phi_r,$dE,$dN,$dU) = @_;
  $dX = -sin($lam_r)*$dE - sin($phi_r)*cos($lam_r)*$dN + cos($phi_r)*cos($lam_r)*$dU;
  $dY =  cos($lam_r)*$dE - sin($phi_r)*sin($lam_r)*$dN + cos($phi_r)*sin($lam_r)*$dU;
  $dZ =  cos($phi_r)*$dN + sin($phi_r)*$dU;
  return($dX,$dY,$dZ);
}
