#!/usr/bin/ruby

=begin
log.dat経由でu-blox受信機から得られた各種情報をプロットする
現在のところターゲットは
1) 可視衛星個数
2) 遅れ情報
=end

$: << '~/src/eclipse/autopilot/common/ruby/'
require 'gnuplot_support'

$: << File::join([File::dirname(__FILE__), "swig"])
require 'build_SWIG/SylphideProcessor.so'

$stderr.puts "Usage: #{__FILE__} [log.dat] [--key[=val]]"

CONFIG_FILE = 'plot_config.rb'
config = {}
ARGV.delete_if{|item|
  next if item !~ /^--([^=]+)=?/
  config[$1.to_sym] = $'
  true
}
if FileTest::exist?(CONFIG_FILE) then
  puts "CONFIG_FILE ON!"
  eval(open(CONFIG_FILE).read)
end
p $config = config

# 単純グラフ
class Plotter
  def Plotter::plot_time_series(file_prefix, t, v, options = {})
    return unless t
    Plotter::plot_basic("#{file_prefix}.eps"){|plot|
      if !$config[:LABEL_FORMAT_E] then
        plot.xlabel "'GPS Time [{/Symbol \264}10^{8} msec]'" ##
        plot.set('format', 'x "%3.2t"') ##
      else
        plot.xlabel "'GPS Time [msec]'"
      end
      
      plot.set("ylabel \"#{options[:ylabel]}\"") if options[:ylabel]
      
      if $config[:SMALL_PLOT] then 
        plot.set('size', "#{$config[:SMALL_PLOT]}")
      end
      
      plot.xrange <<-__STRING__
[#{(t.first / 10).to_i * 10}:#{((t.last / 10).to_i + 1) * 10}]
      __STRING__
      
      if options[:yrange] then
        #options[:yrange] =~ /\[([^:]+):([^\]]+)\]/
        #range = ($2.to_i - $1.to_i)
        #plot.yrange "[#{$1.to_i - range * 0.05}:#{$2.to_i + range * 0.05}]"
        plot.yrange options[:yrange]
      end
      
      yield plot if block_given?
    
      items = [
          Gnuplot::DataSet::new([t, v]){|ds|
            ds.with = "lines lw 3"
            ds.notitle
          }
        ]
      
      plot.data = items
    }
  end
end

# 時刻 => 内容 が入っているハッシュ
$gpsdata = {}
$current_time = 0
$before_first_fix = true

$grabber_g = proc{|observer|
  case observer
  when SylphideProcessor::APacketObserver
    $current_time = observer.fetch_ITOW
  when SylphideProcessor::GPacketObserver
    append_obj = {}
    case observer.ubx_class
    when 0x01
      case observer.ubx_id
      when 0x02
        append_obj[:pos] = observer.position
        append_obj[:pos_acc] = observer.position_acc
      when 0x12
        append_obj[:vel] = observer.velocity
        append_obj[:vel_acc] = observer.velocity_acc
      when 0x03
        #puts "#{observer.status.inspect} at #{observer.status.time_to_reset_ms} ms"
        append_obj[:fix_type] = observer.status.fix_type
        if $before_first_fix \
            and append_obj[:fix_type] >= SylphideProcessor::GStatus::FIX_2D then
          $before_first_fix = false
        end
      end
    when 0x02
      case observer.ubx_id
      when 0x10
        append_obj[:sv] = observer.num_of_sv
      end
    end
    
    # 2D_FIXし始めた時点らのデータを使うようにする
    return if $before_first_fix or append_obj.empty? 
    append_obj[:itow] = observer.fetch_ITOW
    $gpsdata[$current_time] ||= {}
    $gpsdata[$current_time].update(append_obj)
  end
}

$grabber_n = proc{|observer|
  case observer
  when SylphideProcessor::APacketObserver
    $current_time = observer.fetch_ITOW
  when SylphideProcessor::NPacketObserver 
    $gpsdata[$current_time] = {:itow => observer.fetch_ITOW, :pos => observer.navdata}
  end
}

def process_stream(input)
  processor = SylphideProcessor::SylphideLog::new(1024)
  pages = 0
  grabber = $grabber_g
  grabber = $grabber_n if $config.include?(:mode_n)
  while !input.eof? #30000.times{|i|
    pages += 1
    processor.process(input.read(32)){|observer|
      grabber.call(observer)
    }
    $stderr.puts "#{pages} pages processed" if (pages % 10000) == 0
  end #}
end

# ログファイルが指定されていないければ、標準入力から読み込むようにする
if (ARGV.size < 1) || (ARGV.first == '-') then
  $stderr.puts "reading from [stdin] ..."
  process_stream($stdin)
else
  $stderr.puts "reading from #{ARGV[0]} ..."
  open(ARGV.shift, 'r'){|io|
    process_stream(io)
  }
end

if $config.include?(:"start-gpst") then
  threshold = $config[:"start-gpst"].to_f
  $gpsdata.reject!{|t, item| t < threshold}
end

if $config.include?(:"end-gpst") then
  threshold = $config[:"end-gpst"].to_f
  $gpsdata.reject!{|t, item| t > threshold}
end

#p $gpsdata.size; exit

# 実際にグラフ描く処理はここから
# 1) 可視衛星個数
itow_sv = {}
$gpsdata.each{|t, item|
  itow_sv[item[:itow]] = item[:sv] if item.include?(:sv)
}
itow_sv = itow_sv.to_a.sort!{|a, b| a[0] <=> b[0]}.transpose 
Plotter::plot_time_series(\
    "visible", itow_sv[0], itow_sv[1], \
    {:yrange => "[0:16]", :ylabel => "Visible Satellites"})

# 2) 遅れ情報
itow_delay = {}
$gpsdata.each{|t, item|
  itow_delay[item[:itow]] = (t - item[:itow]) if item.include?(:pos)
}
itow_delay = itow_delay.to_a.sort!{|a, b| a[0] <=> b[0]}.transpose 
Plotter::plot_time_series(\
    "delay", itow_delay[0], itow_delay[1], \
    {:yrange => "[0:0.5]", :ylabel => "Delay [sec]"})