#!/usr/bin/ruby

require 'rubygems'
require 'kml'

=begin
KML出力を行う
$ gem install schleyfox-ruby_kml --source http://gems.github.com
$ gem install builder
が必要
=end

$options = {
  :in_mode => :raw,
  :use_alt =>true,
  :start_alt_is_zero => true, 
  :trunk => 1,
  :mod_geoide => true,
  :object => nil,
}

ARGV.reject!{|arg|
  if arg =~ /--([^=]+)=?/ then
    k = $1.to_sym
    v = $'
    case k
    when :in_mode
      $options[k] = v.to_sym
    when :use_alt, :trunk, :mod_geoide, :object
      $options[k] = eval(v)
    when :start_gpstime, :end_gpstime
      $options[k] = v.to_f
    else
      $options[k] = v
    end
    true
  else
    false
  end
}

in_file = ARGV.shift || '-'

file_open_proc = Proc::new{|in_file, &block|
  if in_file == '-' then
    block.call($stdin)
  else
    open(in_file, 'r'){|io|
      block.call(io)
    }
  end
}

itow = []
coordinates = []
attitude = []
file_open_proc.call(in_file){|io|
  case $options[:in_mode]
  when :analyze # analyze.exeの出力を読み込む場合
    $: << File::dirname(__FILE__)
    require 'sylph_processed_read'
    alias :open_orig :open
    def open(arg); return arg; end
    data = SylphReader::read(io).transpose
    alias :open :open_orig
    itow = data[SylphReader::SYLPH_LABELS::index('ITOW')]
    lat_index, lng_index, alt_index = ['latitude', 'longitude', 'height'].collect{|label|
      SylphReader::SYLPH_LABELS::index(label)
    }
    coordinates = [data[lng_index], data[lat_index]]
    coordinates << data[alt_index] if $options[:use_alt]
    coordinates = coordinates.transpose
    attitude = ['Yaw', 'Pitch', 'Roll'].collect{|label|
      data[SylphReader::SYLPH_LABELS::index(label)]
    }.transpose
  when :gaia # GAIAのPVAデータの読み込む場合
    $: << File::join(File::dirname(__FILE__), '..', '060609', 'analysis')
    require 'gaia_reader'
    alias :open_orig :open
    def open(arg); return arg; end
    data = GaiaReader::read(io).transpose
    alias :open :open_orig
    itow = data[GaiaReader::GAIA_LABELS::index('Time')]
    lat_index, lng_index, alt_index = ['Latitude', 'Longitude', 'Altitude'].collect{|label|
      GaiaReader::GAIA_LABELS::index(label)
    }
    coordinates = [data[lng_index], data[lat_index]]
    coordinates << data[alt_index] if $options[:use_alt]
    coordinates = coordinates.transpose
    attitude = ['Yaw', 'Pitch', 'Roll'].collect{|label|
      data[GaiaReader::GAIA_LABELS::index(label)]
    }.transpose
  when :log # logを直接読み込む場合
    $: << File::join(File::dirname(__FILE__), 'swig', 'build_SWIG')
    require 'SylphideProcessor.so'
    log_processor = SylphideProcessor::SylphideLog::new(1024)
    while !io.eof?
      log_processor.process(io.read(32)){|observer|
        case observer
        when SylphideProcessor::NPacketObserver
          navdata = observer.navdata
          next if (navdata.latitude == 0 && navdata.longitude == 0)
          itow << navdata.itow
          data = [navdata.longitude, navdata.latitude]
          data << navdata.altitude if $options[:use_alt]
          coordinates << data
          attitude << [navdata.heading, navdata.pitch, navdata.roll]
        end
      }
    end
  else # strip_for__matlab.rbでそぎ落としたCSV形式
    #line_count = 0
    io.each{|line|
      #line_count += 1
      data = line.split(/,/).collect{|item| item.to_f}
      itow << data[0].to_f
      coordinates << data[1..($options[:use_alt] ? 3 : 2)]
      attitude << data[7..9]
    }
  end
}

if $options[:start_gpstime] then
  index = 0
  itow.each_with_index{|t, i|
    if t >= $options[:start_gpstime] then
      index = i
      break
    end 
  }
  itow = itow[index..-1]
  coordinates = coordinates[index..-1]
  attitude = attitude[index..-1]
end

if $options[:end_gpstime] then
  index = -1
  itow.each_with_index{|t, i|
    if t >= $options[:end_gpstime] then
      index = i
      break
    end 
  }
  itow = itow[0..index]
  coordinates = coordinates[0..index]
  attitude = attitude[0..index]
end

if $options[:trunk] > 1 then
  count = 0
  coordinates.reject!{|item|
    count = (count + 1) % $options[:trunk]
    count != 0
  }
  count = 0
  attitude.reject!{|item|
    count = (count + 1) % $options[:trunk]
    count != 0
  }
end

# WGS84高度からジオイド高度を引いて標高に変換する
if $options[:use_alt] && $options[:mod_geoide] then
  def get_geoide(lng, lat)
    res = 0
    require "net/http"
    require "uri"
    uri = URI.parse("http://vldb.gsi.go.jp/sokuchi/geoid/calcgh/cgi/geoidcalc.pl")
    Net::HTTP.start(uri.host, uri.port){|http|  
      #リクエストインスタンス生成  
      request = Net::HTTP::Post.new(uri.path)  
      #ヘッダー部  
      #request["user-agent"] = "Ruby/#{RUBY_VERSION} MyHttpClient"  
      #ボディ部  #第二引数は省略可能
      #$stderr.puts lat, lng
      form_data = { \
          :ido1 => lat.to_i, :ido2 => (lat * 60).to_i % 60, :ido3 => (lat * 3600).to_i % 60,
          :kei1 => lng.to_i, :kei2 => (lng * 60).to_i % 60, :kei3 => (lng * 3600).to_i % 60}
      #$stderr.puts form_data.inspect
      request.set_form_data(form_data, "&")  
      #送信  
      response = http.request(request)
      #$stderr.puts response
      if response.body =~ /([\+\-\d\.]+) *\(m\)/ then
        res = $1.to_f
      end
    }
    return res
  end
  geoide = get_geoide(*(coordinates[0][0..1])) # スタート地点の座標のジオイド高を参照値にする
  $stderr.puts "Geoide: #{geoide} [m] @ #{coordinates[0][0..1].join(', ')}"
  coordinates.each{|item| item[2] -= geoide}
end

dae_link = KML::Link.new
dae_link.href = "meister.dae"
dae_link.refresh_mode = "once"

features = [
  KML::Placemark.new(
    :name => 'Flight path',
    :description => 'Flight path',
    :style_url => '#transBlue',
    :geometry => KML::LineString.new(
      :extrude => true,
      :tessellate => true,
      :altitude_mode => ($options[:use_alt] ? 'absolute' : 'clampToGround'),
  :coordinates => coordinates.collect{|item| item.join(',')}.join(' '))),
]

if $options[:object] then
  attitude.each_with_index{|att, i|
    #next unless i % 5 == 0
    model = KML::Model.new(
        :location => coordinates[i],
        :orientation => [att[0], att[1], -att[2]], # KMLは左手系 @see https://developers.google.com/kml/documentation/kmlreference#model
        :link => dae_link)
    model.altitude_mode = ($options[:use_alt] ? 'absolute' : 'clampToGround') # ライブラリの初期化方法に問題があるので、こういう書き方で問題回避
    features << KML::Placemark.new(
        :name => 'Attitude',
        :description => 'Attitude',
        :geometry => model)
  }
end

kml = KMLFile.new
kml.objects << KML::Document.new(
  :name => "#{in_file}",
  :description => "#{in_file}",
  :styles => [
    KML::Style.new(
      :id => 'transBlue',
      :line_style => KML::LineStyle.new(:color => '7fff0000', :width => ($options[:use_alt] ? 1.5 : 10)))
  ],
  :features => features
)
puts kml.render
#coordinates.zip(attitude).each{|item| puts item.flatten.join(',')}

__END__
# kmlライブラリを使う際のサンプル
def test_paths
  kml = KMLFile.new
  kml.objects << Document.new(
    :name => 'Paths',
    :description => 'Examples of paths. Note that the tessellate tag is by default
          set to 0. If you want to create tessellated lines, they must be authored
          (or edited) directly in KML.',
    :styles => [
      Style.new(
        :id => 'yellowLineGreenPoly',
        :line_style => LineStyle.new(:color => '7f00ffff', :width => 4),
        :poly_style => PolyStyle.new(:color => '7f00ff00')
      )
    ],
    :features => [
      Placemark.new(
        :name => 'Absolute Extruded',
        :description => 'Transparent green wall with yellow outlines',
        :style_url => '#yellowLineGreenPoly',
        :geometry => LineString.new(
          :extrude => true,
          :tessellate => true,
          :altitude_mode => 'absolute',
          :coordinates => '-112.2550785337791,36.07954952145647,2357
                    -112.2549277039738,36.08117083492122,2357
                    -112.2552505069063,36.08260761307279,2357
                    -112.2564540158376,36.08395660588506,2357
                    -112.2580238976449,36.08511401044813,2357
                    -112.2595218489022,36.08584355239394,2357
                    -112.2608216347552,36.08612634548589,2357
                    -112.262073428656,36.08626019085147,2357
                    -112.2633204928495,36.08621519860091,2357
                    -112.2644963846444,36.08627897945274,2357
                    -112.2656969554589,36.08649599090644,2357'
        )
      )
    ]
  )
  puts kml.render
  #write_and_show(kml, File.dirname(__FILE__) + '/paths.kml')
end