#!/opt/perl/bin/perl
use GOA::Util;
use GPS::Geodesy;
use GPS::DATES qw(get_date sec2cal);
use GOA::TdpSol;
use Matrix;
use Cwd;

my $dir = shift || cwd;
my $smsol_file = "$dir/xyz_smsol.txt";
my $tdp_file = "$dir/tdp_final";
my $corr_file = "$dir/tdp_corr_final";

my $sol = new GOA::TdpSol;

$sol->read_corr($corr_file);


#exit;

$sol->read_smsol($smsol_file);
my $epoch = $sol->{sec_epoch};
my $cal = get_date($sol->{epoch},'cal,time');
my $doy = get_date($cal,'doy');
my $gwd = get_date($cal,'gwd');
my $date = 
my $s=$sol->xyz;
my $site = lc $sol->site;
my $cc=$sol->cc;
my @nom_xyz = $sol->nxyz;
my ($lat,$lon,$ht) = xyz2llh(@nom_xyz);
my @cov=();
my @comp = qw(X Y Z);
my ($i,$j,$rho,$sigi,$sigj,$env,$cov_env,$xyz,@sig);

my $header=qq/# Site: $site
# Date: $cal   ; SecJ2000: $epoch ; WWWWD: $gwd ; Doy: $doy
# Nom. XYZ : @nom_xyz
# Lat: $lat
# Lon: $lon
# Height: $ht 
/;

open(my $eh,'>', "$site.lon" ) or die "Coudln't open $site.lon for writing $!\n";
open(my $nh,'>', "$site.lat" ) or die "Coudln't open $site.lat for writing $!\n";
open(my $vh,'>', "$site.rad" ) or die "Coudln't open $site.rad for writing $!\n";
open( my $fh, '>', "$site.envcov") or die "Couldn't open $site.envcov $!\n";
print $fh $header;
print $eh $header;
print $nh $header;
print $vh $header;
my( @env0,@xyz0);
foreach my $sec ( sort { $a <=> $b } keys %{$s} ) {
  #print map { "$_ " } ( $sec/86400, $s->{$sec}{X}{val}, $s->{$sec}{X}{sig},"\n");
  $v=$s->{$sec};
  $c=$cc->{$sec};
  @xyz = map { $v->{$_}{val} } @comp;
  my $xyz = new Matrix(@xyz);
  my $nxyz = new Matrix(@nom_xyz);
  $xyz = $xyz - $nxyz;
  @sig_xyz = map { $v->{$_}{sig} } @comp ;
  for  $i ( 0 .. 2 ) {
    $sigi=$sig_xyz[$i];
    for $j ( 0 .. $i ) {
      $sigj=$sig_xyz[$j];
      $rho = $i == $j ? 1.0 :   $c->{$comp[$i]}{$comp[$j]} 
                            ||  $c->{$comp[$j]}{$comp[$i]};
             
      $cov[$i][$j]=$rho*$sigi*$sigj;   
      if ( $i != $j ) {
        $cov[$j][$i]=$cov[$i][$j];
      }
    }
  }
 ($env,$cov_env)=xyz2env(\@nom_xyz,$xyz,\@cov);
  @sig= map { sqrt($$cov_env[$_][$_]) } ( 0 .. 2 );
  @$env = map { $_*1e3 } @$env;
  #@env0 = (0,0,0); #@$env if ! defined @env0;
  $t = $sec; # - $epoch; #sprintf "%.5f",($sec-$epoch)/3600;
  print $eh join ' ',($t, $$env[0], $sig[0]*1e3,"\n");
  print $nh join ' ',($t, $$env[1], $sig[1]*1e3,"\n");
  print $vh join ' ',($t, $$env[2], $sig[2]*1e3,"\n");
  print $fh join ' ', (  $t,
                         $$env[0], 
                         $$env[1], 
                         $$env[2], 
                         $sig[0]*1e3,
                         $sig[1]*1e3,
                         $sig[2]*1e3,
                         $$cov_env[0][1]/$sig[0]/$sig[1],
                         $$cov_env[0][2]/$sig[0]/$sig[2],
                         $$cov_env[1][2]/$sig[1]/$sig[2],
		                 "\n");
}
