#!/usr/bin/perl -w
#
# GPSy拡張形式のトラックログを地図画像ビューア 2.0xで再生
# Nowral
# 01/10/27
#
# 初期化
use Time::Local;
$td = 10; # 
$tdh = 30;
$tint = 600; # 時刻表示、何秒おきか
$cd = 0x10000; # depth / color
$pi  = 4 * atan2(1,1); # 円周率
$rd  = $pi / 180;      # [ラジアン/度]
$btel = "tell application \"地図画像ビューア\"\n";
$etel = "end tell\n";

# 出力ファイル準備
$newargv = "track.as";
open(OUT, ">$newargv");
MacPerl::SetFileInfo('ToyS', 'TEXT', $newargv);
print OUT $btel;

$buff = "\tactivate\n";
MacPerl::DoAppleScript("$btel$buff$etel");
print OUT $buff;

# ログ読み込み
$nt = 0;
$nw = 0;
$n = 0;
$oldargv = "";
my($t1, $b1, $l1, $h1, $t2, $b2, $l2, $h2);
while (<>) {
  @ed = split("\t");
  next unless($ed[0] eq "T" ||
   $ed[0] eq "W" || $ed[0] eq "Wx" || $ed[0] eq "wx");

  unless(defined $dat) {
    if($ed[10] eq "Tokyo") {
      $a   = 6377397.155;    # 赤道半径 (Tokyo)
      $f   = 1 / 299.152813; # 扁平率 (Tokyo)
      $dat = "Tokyo";
    }
    elsif($ed[10] eq "WGS 84" || $dat eq "<NO TRANSLATION>") {
      $a   = 6378137;        # 赤道半径 (WGS 84)
      $f   = 1 / 298.257223; # 扁平率 (WGS 84)
      $dat = "WGS-84";
    }
    else {
      die "$dat Datum Not Supported";
    }
    $e2  = 2*$f - $f*$f;   # 第1離心率
  }

  $b1 = &gdms2deg($ed[5]); # 緯度
  $l1 = &gdms2deg($ed[6]); # 経度
  $h1 = $ed[8] + 0; # 高さ <>楕円体高?

# トラック
  if($ed[0] eq "T") {
    $ed[4] =~ /(\d+)\/(\d+)\/(\d+)\s+(\d+):(\d+):(\d+)/ || die;
    $t1 = timelocal($6,$5,$4,$2,$1-1,$3-1900);

    $trn = "" if(defined $t2 && $t1-$t2>300
     && abs($b1-$b2)<1/3600 && abs($l1-$l2)<1/3600);
    if($ARGV ne $oldargv || $ed[1] ne $trn) {
      unless(defined $t2 && $t1-$t2<300) {
        &prc_trk(\@date, \@lat, \@lon, \@alt, $n-1) if $n>2;
        $oldargv = $ARGV;
        $trn = $ed[1];
        $n = 0;
        undef $dat;
        undef @date;
        undef @lat;
        undef @lon;
        undef @alt;
      }
    }

    push(@date, $t1);
    push(@lat, $b1);
    push(@lon, $l1);
    push(@alt, $h1);
    ($t2, $b2, $l2, $h2) = ($t1, $b1, $l1, $h1);
    ++$n;
  }
# ウェイポイント
  elsif($ed[0] eq "W" || $ed[0] eq "Wx" || $ed[0] eq "wx") {
    $clr = &penprop($nw);
    $buff = <<END_SCRIPT4;
  AddMemo  "DEG $dat" MemoKind 1 ﾂ
    Latitude $b1 Longitude $l1 ﾂ
    MemoName "$ed[2]" Comment "" ﾂ
    NameColor {$clr}
END_SCRIPT4
    MacPerl::DoAppleScript("$btel$buff$etel");
    print OUT $buff;
    ++$nw;
  }
}
&prc_trk(\@date, \@lat, \@lon, \@alt, $n-1) if $n>2;

print OUT $etel;
close(OUT);

printf "# トラック本数    ： %d\n", $nt;
printf "# ウェイポイント数： %d\n", $nw;

#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, $t2, $t3, $u, $tlm);
  my($vx1, $vy1, $vz1, $v1, $vmax, @v, $vlm);
  my($ax1, $ay1, $az1, $ap, @ap);
  my(@t) = @{$tref};
  my($cmnt, $clr, @cmnt, $ic1, $ic2);
  my($i, $buff, $nd, $rt);
  ($t0, $t3) = ($t[0], $t[$n]);
  $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);
  }

# 停止位置検出
  $i = 0;
  $vmax = 0;
  undef @v;
  undef @ap;
  for($t1=$t0; $t1<=$t3; ++$t1) {
    for(; $t[$i+1]<$t1; ++$i) {}
    $u = ($t1-$t[$i]) / $h[$i];
    $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 = sqrt($vx1**2 + $vy1**2 + $vz1**2);
    $vmax = $v1 if $vmax<$v1;
    push(@v, $v1);
    if($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]);
      $ap = $ax1*$vx1 + $ay1*$vy1 + $az1*$vz1; # 加速度の進行方向成分
      push(@ap, $ap/$v1);
    }
    else { push(@ap, 0); }
  }
  undef @cmnt;
  $vlm = 0;
  $t2 = -$tdh;
  for($t1=$tdh; $t1<=$st-$tdh; ++$t1) {
    $v1 = $v[$t1];
    if($vlm<$v1) {
      $vlm = $v1;
      $tlm = $t1;
    }
    if($ap[$t1-1]<0 && $ap[$t1]>0 && $v1<0.5
     && $v[$t1-$tdh]+$v[$t1+$tdh]>8 && $t1-$t2>$tdh*2) {
      $cmnt[int($t1/$td+0.5)] = "●";
      $t2 = $t1;
      $cmnt[int($tlm/$td+0.5)] = sprintf("<%.0f", $vlm*3.6) if $vlm>5;
      $vlm = 0;
    }
  }

# トラック情報
  my($min0, $hour0, $mday0, $mon0) = (localtime($t0))[1..4];
  my($min3, $hour3, $mday3, $mon3) = (localtime($t3))[1..4];
  printf "# 元データ数： %d\n", $n+1;
  printf "# 開始時刻  ： %d:%02d\n", $hour0, $min0;
  printf "# 終了時刻  ： %d:%02d\n", $hour3, $min3;
  printf "# 所要時間  ： %d｡%d\'\n", int($st/3600), $st/60%60;
  if($sr<10000) {
    printf "# 総距離    ： %.0f [m]\n", $sr;
  }
  else {
    printf "# 総距離    ： %.1f [km]\n", $sr/1000;
  }
  printf "# 平均速度  ： %.1f [km/h]\n", $sr/$st*3.6;

# 新軌跡
  $rt = time;
  $clr = &penprop($mday0+$nt);
  $cmnt = sprintf("%d/%d %02d:%02d-", $mon0, $mday0, $hour0, $min0)
   . ($mday0==$mday3 ? "" : "$mday3 ") . sprintf("%02d:%02d", $hour3, $min3);
  $buff = <<END_SCRIPT1;
  beep 1
  NewLine "DEG $dat" LineKind 1 ﾂ
    LineName "$cmnt" LineColor {$clr} ﾂ
    NameColor {$clr} CommentColor {$clr}
END_SCRIPT1
  MacPerl::DoAppleScript("$btel$buff$etel");
  print OUT $buff;

# 書き出しループ
  $i = 0;
  $ic2 = 0;
  $nd = 0;
  $t2 = $t0 + $tint;
  for($t1=$t0; $t1<$t3+$td; $t1+=$td) {
    $t1 = $t3 if $t1>$t3;
    for(; $t[$i+1]<$t1; ++$i) {}
    $u = ($t1-$t[$i]) / $h[$i];
    $x1 = &sp_x($u, $h[$i], $x[$i], $x[$i+1], $ax[$i], $ax[$i+1]);
    $y1 = &sp_x($u, $h[$i], $y[$i], $y[$i+1], $ay[$i], $ay[$i+1]);
    $z1 = &sp_x($u, $h[$i], $z[$i], $z[$i+1], $az[$i], $az[$i+1]);
    my($b1, $l1, $h1) = &xyz2llh($x1, $y1, $z1, $a, $e2);

# アイコン
# 0: アイコンを表示しない(デフォルト)
# 1: 矢印、 2: 歩く人、 3: 自転車、 ﾂ
# 4: 自動車(赤)、 5: 亀、 6: 自動車(青)
    $ic1 = $v[$t1-$t0] * 3.6;
    $ic1 = $ic1<2 ? 5 : $ic1<6 ? 2 : $ic1<25 ? 3 : $ic1<60 ? 6 : $ic1<120 ? 4 : 1;
    $buff = $ic1==$ic2 ? "" : "\tSetIcon $ic1\n";
    $ic2 = $ic1;

# コメント
    $cmnt = defined $cmnt[int(($t1-$t0)/$td)] ? $cmnt[int(($t1-$t0)/$td)] : "";
    if($t1>=$t2 || $t1==$t3) { # 時刻表示のタイミング?
      $sr = $sr[$i] + &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]);
      $cmnt .= " " if $cmnt;
      $cmnt .= ($t1-$t0>=3600 ? (int(($t1-$t0)/3600))."｡" : "")
       . sprintf("%d\' %.1fk", ($t1-$t0)/60%60, $sr/1000);
      $t2 += $tint;
    }
    if($t1==$t0 || $t1==$t3) {
      $cmnt .= " " if $cmnt;
      $cmnt .= sprintf("(%d:%02d)", (localtime($t1))[2,1]);
    }

# ポイント追加
    $buff .= <<END_SCRIPT2;
  AddNode Latitude $b1 Longitude $l1 Comment "$cmnt"
END_SCRIPT2
    MacPerl::DoAppleScript("$btel$buff$etel");
    print OUT $buff;
    $nd++;
  }

# 軌跡終了
  $buff = <<END_SCRIPT3;
  CloseLine
  beep 1
END_SCRIPT3
  MacPerl::DoAppleScript("$btel$buff$etel");
  print OUT $buff;

  $rt = time - $rt;
  printf "# 最高速度  ： %.1f [km/h]\n", $vmax*3.6;
  printf "# 新データ数： %d\n", $nd;
  printf "# 所要時間  ： %d\'%d\"\n", $rt/60%60, $rt%60;
  print "\n";

  ++$nt;
}

sub penprop { # 番号 -> 色
  my($id) = @_;
  my($cb, $cr, @cs);
  $id *= 65;
  $cb = int(($id+60) / 120);
  $cr = ($id/120-$cb) * $cd;
  $cs[$cb%3] = $cd - 1;
  ($cs[($cb+1)%3], $cs[($cb+2)%3]) = $cr>0 ? ($cr, 0) : (0, -$cr);
  return sprintf("%d, %d, %d", @cs);
}

sub gdms2deg { # 経緯度、GPSy形式 -> 度
  my($d) = @_;
  $d =~ /(-?)(\d*\.?\d*)｡?(\d*\.?\d*)'?(\d*\.?\d*)"?/ || die;
  return ($1 ? -1 : 1) * ($2 + $3/60 + $4/3600);
}

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