#!perl -w
#
# GPSy拡張形式のトラックログから速度加速度計算
# Nowral
# 00/12/23
#
# ファイル毎処理?
use Time::Local;
$tint = 0 + eval( MacPerl::Ask(
"何秒おきに書出しますか? (「0」で、元データと同じ時刻で書出し。)"
, "0") );
die if($tint<0);

# 定数 < グローバル
$pi  = 4 * atan2(1, 1); # 円周率
$rd  = $pi / 180;      # [ラジアン/度]
$g = 9.80665;   # 重力加速度 [m/s^2]
# データム諸元 (Tokyo)
$a = 6377397.155; # 赤道半径 [m]
$f = 1 / 299.152813; # 扁平率
$e2 = 2*$f - $f*$f; # 第一離心率

# 出力ファイル準備
$0 =~ /(.+:)/;
$newargv = "$1result.dat";
open(OUT, ">$newargv");
MacPerl::SetFileInfo('ttxt', 'TEXT', $newargv);
# ヘッダ出力
print OUT "経過時間[秒]\t積算距離[km]\t速度[km/h]\t加速度[G]\t方位角[度]\n";

# データ読み込み
$n = 0;
$oldargv = "";
$trn = "";
while (<>) {
  next unless(/^T\t/);
  @ed = split("\t");
  if ($ARGV ne $oldargv || $ed[1] ne $trn) {
    &prc_trk(\@t, \@b, \@l, \@h, $n-1) if $n>=2;
    $oldargv = $ARGV;
    $trn = $ed[1];
    $n = 0;
    undef @t;
    undef @b;
    undef @l;
    undef @h;
  }
# 時刻
  $ed[4] =~ /(\d+)\/(\d+)\/(\d+)\s+(\d+):(\d+):(\d+)/;
  $t2 = timelocal($6,$5,$4,$2,$1-1,$3-1900);
  push(@t, $t2);
# 緯度
  $ed[5] =~ /(-?)(\d+)｡(\d+)'(\d+\.?\d*)"/;
  $b2 = ($1 ? -1 : 1) * ($2+$3/60+$4/3600);
  push(@b, $b2);
# 経度
  $ed[6] =~ /(-?)(\d+)｡(\d+)'(\d+\.?\d*)"/;
  $l2 = ($1 ? -1 : 1) * ($2+$3/60+$4/3600);
  push(@l, $l2);
# 高さ <>楕円体高?
  $h2 = $ed[8];
  push(@h, $h2);

  ++$n;
}
&prc_trk(\@t, \@b, \@l, \@h, $n-1) if $n>=2;
close(OUT);

#MacPerl::Quit(2);
die "The Unhappy End";



sub prc_trk { # 一つながりのトラックを処理
  my($tref, $bref, $lref, $href, $n) = @_;
  my(@x, @y, @z, @h);
  my(@ax, @ay, @az);
  my(@sr, $sr, $dr);
  my($t0, $t1, $t3, $u, $i);
  my($vx1, $vy1, $vz1, $v1);
  my($ax1, $ay1, $az1, $a1);
  my(@tp, $vmax, $amax, $amin);
  my(@t) = @{$tref};
  ($t0, $t3) = ($t[0], $t[$n]);
  $t00 = $t0 unless defined $t00;
  $st = $t3 - $t0;
  foreach $i (0 .. $n) {
    ($x[$i], $y[$i], $z[$i])
     = &llh2xyz(${$bref}[$i], ${$lref}[$i], ${$href}[$i], $a, $e2);
  }
  foreach $i (0 .. $n-1) { $h[$i] = $t[$i+1] - $t[$i]; }
  @ax = &sp_gen(\@x, \@h, $n);
  @ay = &sp_gen(\@y, \@h, $n);
  @az = &sp_gen(\@z, \@h, $n);
# 積算距離計算
  $sr = 0;
  undef @sr;
  push(@sr, $sr);
  foreach $i (0 .. $n-1) {
    $dr = &part_amt(1, $h[$i],
     $x[$i], $x[$i+1], $ax[$i], $ax[$i+1],
     $y[$i], $y[$i+1], $ay[$i], $ay[$i+1],
     $z[$i], $z[$i+1], $az[$i], $az[$i+1]);
    $sr += $dr;
    push(@sr, $sr);
  }

# 書き出し時刻
  undef @tp;
  if($tint) { 
    for($t1=$t0; $t1<$t3; $t1+=$tint) { push(@tp, $t1); }
    push(@tp, $t3);
  }
  else { @tp = @t; }
#  printf OUT "# 開始時刻  ： %s\n", &formd($t0);

#集計
  print "\n";
  printf "# 元データ数： %d\n", $n+1;
  printf "# 出力行数  ： %d\n", $#tp+1;
  printf "# 開始時刻  ： %s\n", &formd($t0);
  printf "# 終了時刻  ： %s\n", &formd($t3);
  printf "# 所要時間  ： %d:%02d:%02d\n", int($st/3600), ($st/60)%60, $st%60;
  printf "# 総距離    ： %.1f [km]\n", $sr/1000;
  printf "# 平均速度  ： %.1f [km/h]\n", $sr/$st*3.6;

# 書き出しループ
  $i = 0;
  ($vmax, $amax, $amin) = (0, 0, 0);
  foreach $t1 (@tp) {
    while($t[$i+1]<$t1) { ++$i; }
    $u = ($t1-$t[$i]) / $h[$i];
# 積算距離
    $dr = &part_amt($u, $h[$i],
     $x[$i], $x[$i+1], $ax[$i], $ax[$i+1],
     $y[$i], $y[$i+1], $ay[$i], $ay[$i+1],
     $z[$i], $z[$i+1], $az[$i], $az[$i+1]);
    $sr = $sr[$i] + $dr;
# 速度
    $vx1 = &sp_v($u, $h[$i], $x[$i], $x[$i+1], $ax[$i], $ax[$i+1]);
    $vy1 = &sp_v($u, $h[$i], $y[$i], $y[$i+1], $ay[$i], $ay[$i+1]);
    $vz1 = &sp_v($u, $h[$i], $z[$i], $z[$i+1], $az[$i], $az[$i+1]);
    $v1 = &absvec($vx1, $vy1, $vz1);
    $vmax = $v1 if $vmax<$v1;
# 加速度
    $ax1 = &sp_a($u, $ax[$i], $ax[$i+1]);
    $ay1 = &sp_a($u, $ay[$i], $ay[$i+1]);
    $az1 = &sp_a($u, $az[$i], $az[$i+1]);
    $a1 = &absvec($ax1, $ay1, $az1);
    $a1 *= (&inprod($ax1, $ay1, $az1, $vx1, $vy1, $vz1)>0 ? 1 : -1);
    $amax = $a1 if $amax<$a1;
    $amin = $a1 if $amin>$a1;
# 方位角
    $azm = &azimuth($x[$i], $y[$i], $z[$i], $vx1, $vy1, $vz1);
# 書き出し
    printf OUT "%d\t%.3f\t%.1f\t%.3f\t%.0f\n",
     $t1-$t00, $sr/1000, $v1*3.6, $a1/$g, $azm;
  }
  print OUT "\n";

#集計
  printf "# 最高速度  ： %.1f [km/h]\n", $vmax*3.6;
  printf "# 最大加速  ： %.2f [G]\n", $amax/$g;
  printf "# 最大減速  ： %.2f [G]\n", -$amin/$g;
  print "\n";

  return 1;
}

sub formd { # 日付、時刻の表示形式
  my($t) = @_;
  my($sec,$min,$hour,$mday,$mon,$year,$wday,$yday,$isdst) = localtime($t);
  return sprintf("%d:%02d", $hour, $min);
#  return sprintf("%04d/%d/%d %d:%02d:%02d", $year+1900,$mon+1,$mday,$hour,$min,$sec);
}

sub llh2xyz { # 楕円体座標 -> 直交座標
  my($b, $l, $h, $a, $e2) = @_;
  my($sb, $cb, $rn, $x, $y, $z);

  $b *= $rd;
  $l *= $rd;
  $sb = sin($b);
  $cb = cos($b);
  $rn = $a / sqrt(1-$e2*$sb*$sb);

  $x = ($rn+$h) * $cb * cos($l);
  $y = ($rn+$h) * $cb * sin($l);
  $z = ($rn*(1-$e2)+$h) * $sb;

  ($x, $y, $z);
}

sub xyz2llh { # 直交座標 -> 楕円体座標
  my($x, $y, $z, $a, $e2) = @_;
  my($bda, $p, $t, $st, $ct, $b, $l, $sb, $rn, $h);
  $bda = sqrt(1-$e2); # b/a

  $p = sqrt($x*$x+$y*$y);
  $t = atan2($z, $p*$bda);
  $st = sin($t);
  $ct = cos($t);
  $b = atan2($z+$e2*$a/$bda*$st*$st*$st, $p-$e2*$a*$ct*$ct*$ct);
  $l = atan2($y, $x);

  $sb = sin($b);
  $rn = $a / sqrt(1-$e2*$sb*$sb);
  $h = $p/cos($b) - $rn;

  ($b/$rd, $l/$rd, $h);
}

sub sp_gen { # 3次の自然スプライン決定
  my($xref, $href, $n) = @_;
  my(@x) = @{$xref};
  my(@h) = @{$href};

  my(@rho, @tau);
  $rho[1] = 0;
  $tau[1] = 0;
  foreach $i (1 .. $n-1) {
    my($di) = 6 * (($x[$i+1]-$x[$i])/$h[$i] - ($x[$i]-$x[$i-1])/$h[$i-1]);
    my($fi) = $h[$i-1]*$rho[$i] + 2*$h[$i] + 2*$h[$i-1];
    $rho[$i+1] = - $h[$i] / $fi;
    $tau[$i+1] = ($di - $h[$i-1]*$tau[$i]) / $fi;
  }
  undef @x;
  undef @h;

  my(@a);
  $a[$n] = 0;
  for($i=$n; $i>0; --$i) { $a[$i-1] = $rho[$i]*$a[$i] + $tau[$i]; }

  return @a;
}

sub sp_x { # スプライン関数値の計算
  my($u, $hi, $xi, $xip, $ai, $aip) = @_;
  my($uu) = 1 - $u;
  my($hh6) = $hi * $hi / 6;
  return $hh6*$ai*$uu*$uu*$uu + $hh6*$aip*$u*$u*$u
   + ($xi-$hh6*$ai)*$uu + ($xip-$hh6*$aip)*$u;
}

sub sp_v { # スプライン関数一階微分値の計算
  my($u, $hi, $xi, $xip, $ai, $aip) = @_;
  my($uu) = 1 - $u;
  return - $ai*$hi/2*$uu*$uu + $aip*$hi/2*$u*$u
   + ($xip-$xi)/$hi - $hi/6*($aip-$ai);
}

sub sp_a { # スプライン関数二階微分値の計算
  my($u, $ai, $aip) = @_;
  return $ai*(1-$u) + $aip*$u;
}

sub dldt { # 速度の絶対値 <> 単位時間あたりに進む距離
  my($u, $hi,
   $xi, $xip, $axi, $axip,
   $yi, $yip, $ayi, $ayip,
   $zi, $zip, $azi, $azip) = @_;
  my($vx, $vy, $vz);
  $vx = &sp_v($u, $hi, $xi, $xip, $axi, $axip);
  $vy = &sp_v($u, $hi, $yi, $yip, $ayi, $ayip);
  $vz = &sp_v($u, $hi, $zi, $zip, $azi, $azip);
  return sqrt($vx*$vx + $vy*$vy + $vz*$vz);
}

sub part_amt { # i区間におけるパラメタuまでの積算
  my($u, $hi,
   $xi, $xip, $axi, $axip,
   $yi, $yip, $ayi, $ayip,
   $zi, $zip, $azi, $azip) = @_;
  my($sr, $m, $k);
  $m = $u * $hi;
  $sr = &dldt(0, $hi, $xi, $xip, $axi, $axip, $yi, $yip, $ayi, $ayip, $zi, $zip, $azi, $azip);
  for($k=1; $k<$m; ++$k) {
    $sr += 2 * &dldt($k/$hi, $hi, $xi, $xip, $axi, $axip, $yi, $yip, $ayi, $ayip, $zi, $zip, $azi, $azip);
    $sr += 4 * &dldt($k/$hi-0.5, $hi, $xi, $xip, $axi, $axip, $yi, $yip, $ayi, $ayip, $zi, $zip, $azi, $azip);
  }
  $k--;
  $m -= $k;
  $sr += ($m-1) * &dldt($k/$hi, $hi, $xi, $xip, $axi, $axip, $yi, $yip, $ayi, $ayip, $zi, $zip, $azi, $azip);
  $sr += 4 * $m * &dldt(($k+$m/2)/$hi, $hi, $xi, $xip, $axi, $axip, $yi, $yip, $ayi, $ayip, $zi, $zip, $azi, $azip);
  $sr += $m     * &dldt($u, $hi, $xi, $xip, $axi, $axip, $yi, $yip, $ayi, $ayip, $zi, $zip, $azi, $azip);

  return $sr/6;
}

sub azimuth { # 方位角計算
  my($x, $y, $z, $vx, $vy, $vz) = @_;
# 法線ベクトル
  my($b, $l, $h) = &xyz2llh($x, $y, $z, $a, $e2);
  $b *= $rd;
  $l *= $rd;
  my($nx, $ny, $nz) = (cos($b)*cos($l), cos($b)*sin($l), sin($b));
# 速度ベクトルの地平面射影
  my($vpx, $vpy, $vpz) = &outprod($nx, $ny, $nz, $vx, $vy, $vz);
# 北極ベクトルの地平面射影
  my($ppx, $ppy, $ppz) = &outprod($nx, $ny, $nz, 0, 0, 1);
# 方位角
  my($ax, $ay, $az) = &outprod($ppx, $ppy, $ppz, $vpx, $vpy, $vpz);
  my($a1) = (&inprod($ax, $ay, $az, $nx, $ny, $nz)>0 ? -1 : 1) * # 北から時計回り
   atan2(&absvec($ax, $ay, $az), &inprod($ppx, $ppy, $ppz, $vpx, $vpy, $vpz));
  $a1 += 2*$pi if $a1<0;

  return $a1/$rd;
}

sub absvec { # ベクトルの絶対値
  my($ax, $ay, $az) = @_;
  return sqrt($ax*$ax + $ay*$ay + $az*$az);
}

sub inprod { # ベクトルの内積
  my($ax, $ay, $az, $bx, $by, $bz) = @_;
  return $ax*$bx + $ay*$by + $az*$bz;
}

sub outprod { # ベクトルの外積
  my($ax, $ay, $az, $bx, $by, $bz) = @_;
  return ($ay*$bz-$az*$by, $az*$bx-$ax*$bz, $ax*$by-$ay*$bx);
}